diff --git a/samples/tutorials/data/get_imu_correspondence.cc b/samples/tutorials/data/get_imu_correspondence.cc index 77d3398..7faef67 100644 --- a/samples/tutorials/data/get_imu_correspondence.cc +++ b/samples/tutorials/data/get_imu_correspondence.cc @@ -55,76 +55,77 @@ int main(int argc, char *argv[]) { auto &&left_data = api->GetStreamData(Stream::LEFT); auto &&right_data = api->GetStreamData(Stream::RIGHT); - if (left_data.frame.empty() || right_data.frame.empty()) { - continue; - } + if (!left_data.frame.empty() || !right_data.frame.empty()) { + auto img_stamp = left_data.img->timestamp; + LOG(INFO) << "Img timestamp: " << img_stamp + << ", diff_prev=" << (img_stamp - prev_img_stamp); + prev_img_stamp = img_stamp; - auto img_stamp = left_data.img->timestamp; - LOG(INFO) << "Img timestamp: " << img_stamp - << ", diff_prev=" << (img_stamp - prev_img_stamp); - prev_img_stamp = img_stamp; + cv::Mat img; + cv::hconcat(left_data.frame, right_data.frame, img); - cv::Mat img; - cv::hconcat(left_data.frame, right_data.frame, img); - - auto &&motion_datas = api->GetMotionDatas(); - LOG(INFO) << "Imu count: " << motion_datas.size(); - for (size_t i = 0, n = motion_datas.size() - 1; i <= n; ++i) { - auto data = motion_datas[i]; - auto imu_flag = data.imu->flag; - auto imu_stamp = data.imu->timestamp; - - std::stringstream ss; - if (imu_flag == 0) { // accel + gyro - ss << "Imu"; - } else if (imu_flag == 1) { // accel - ss << "Accel"; - } else if (imu_flag == 2) { // gyro - ss << "Gyro"; + auto &&motion_datas = api->GetMotionDatas(); + LOG(INFO) << "Imu count: " << motion_datas.size(); + if (motion_datas.size() == 0) { + continue; } - ss << " timestamp: " << imu_stamp - << ", diff_prev=" << (imu_stamp - prev_imu_stamp) - << ", diff_img=" << (1.0f + imu_stamp - img_stamp); -#ifdef CHECK_ACCEL_THEN_GYRO - if (imu_flag > 0) { - bool ok = false; - if (i == 0) { // first - ok = (imu_flag == SEQ_FIRST); - } else if (i == n) { // last - ok = (imu_flag == SEQ_SECOND); - } else { - if (imu_flag == SEQ_FIRST) { - ok = (prev_imu_flag == SEQ_SECOND); - } else if (imu_flag == SEQ_SECOND) { - ok = (prev_imu_flag == SEQ_FIRST); + for (size_t i = 0, n = motion_datas.size() - 1; i <= n; ++i) { + auto data = motion_datas[i]; + auto imu_flag = data.imu->flag; + auto imu_stamp = data.imu->timestamp; + + std::stringstream ss; + if (imu_flag == 0) { // accel + gyro + ss << "Imu"; + } else if (imu_flag == 1) { // accel + ss << "Accel"; + } else if (imu_flag == 2) { // gyro + ss << "Gyro"; + } + ss << " timestamp: " << imu_stamp + << ", diff_prev=" << (imu_stamp - prev_imu_stamp) + << ", diff_img=" << (1.0f + imu_stamp - img_stamp); + #ifdef CHECK_ACCEL_THEN_GYRO + if (imu_flag > 0) { + bool ok = false; + if (i == 0) { // first + ok = (imu_flag == SEQ_FIRST); + } else if (i == n) { // last + ok = (imu_flag == SEQ_SECOND); + } else { + if (imu_flag == SEQ_FIRST) { + ok = (prev_imu_flag == SEQ_SECOND); + } else if (imu_flag == SEQ_SECOND) { + ok = (prev_imu_flag == SEQ_FIRST); + } + } + ss << (ok ? " ✓" : " x"); + if (!ok) ++imu_disorder_count; + prev_imu_flag = imu_flag; + if (!exit) { + if (!ok) exit = true; } } - ss << (ok ? " ✓" : " x"); - if (!ok) ++imu_disorder_count; - prev_imu_flag = imu_flag; - if (!exit) { - if (!ok) exit = true; - } + #endif + LOG(INFO) << ss.str(); + + prev_imu_stamp = imu_stamp; } -#endif - LOG(INFO) << ss.str(); + LOG(INFO); + #ifdef CHECK_ACCEL_THEN_GYRO + imu_count += motion_datas.size(); + if (exit) break; + #endif - prev_imu_stamp = imu_stamp; + /* + painter.DrawImgData(img, *left_data.img); + if (!motion_datas.empty()) { + painter.DrawImuData(img, *motion_datas[0].imu); + } + */ + + cv::imshow("frame", img); } - LOG(INFO); -#ifdef CHECK_ACCEL_THEN_GYRO - imu_count += motion_datas.size(); - if (exit) break; -#endif - - /* - painter.DrawImgData(img, *left_data.img); - if (!motion_datas.empty()) { - painter.DrawImuData(img, *motion_datas[0].imu); - } - */ - - cv::imshow("frame", img); char key = static_cast(cv::waitKey(1)); if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q