diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index 113030ab..9d47f5b7 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -39,8 +39,6 @@ // https://github.com/JanuszBedkowski/mandeye_controller The output is a session proving trajekctory and point clouds that can be further // processed by "multi_view_tls_registration" program. -// #define SAMPLE_PERIOD (1.0 / 200.0) - std::string winTitle = std::string("drag_folder_with_mandeye_data_and_drop_here-precision_forestry ") + HDMAPPING_VERSION_STRING; std::vector infoLines = { diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index e7de94fd..7a1f0653 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -317,10 +317,23 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa double avg_dt = 1.0 / 200.0; if (imu_data.size() >= 2) { - double t0 = std::get<0>(imu_data.front()).first; - double t1 = std::get<0>(imu_data.back()).first; + const double t0 = std::get<0>(imu_data.front()).first; + const double t1 = std::get<0>(imu_data.back()).first; + const double duration = t1 - t0; + if (t1 > t0) - avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); + avg_dt = duration / static_cast(imu_data.size() - 1); + + if (duration > 24 * 60 * 60) + { + std::cerr << "ERROR: Session is absurdly long : start time : " << t0 << ", end time " << t1 << std::endl; + std::cerr << "ERROR: Setting rate to " << 1.0 / 200.0 << std::endl; + avg_dt = 1.0 / 200.0; + } + } + if (std::fabs(avg_dt - 1.0 / 200.0) > 0.01) + { + std::cerr << "WARNING: The dt found is strange : " << avg_dt << std::endl; } VQFParams vqf_params = buildVQFParams(params); diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 4035ab6f..8a64aa11 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -12,8 +12,6 @@ #include #include -// #define SAMPLE_PERIOD (1.0 / 200.0) - using Trajectory = std::map>; using Imu = std::vector, Eigen::Vector3f, Eigen::Vector3f>>; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index e7378fef..73a55901 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -38,8 +38,6 @@ // https://github.com/JanuszBedkowski/mandeye_controller The output is a session proving trajekctory and point clouds that can be further // processed by "multi_view_tls_registration" program. -// #define SAMPLE_PERIOD (1.0 / 200.0) - std::string winTitle = std::string("Step 1 (Lidar odometry) ") + HDMAPPING_VERSION_STRING; std::vector infoLines = { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index 8bdd3a59..e86efcab 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2301,22 +2301,23 @@ bool process_worker_step_update_rgd_after( double translation_change = (current_m.translation() - last_m.translation()).norm(); - //std::cout << "translation_change: " << translation_change << std::endl; + // std::cout << "translation_change: " << translation_change << std::endl; if (translation_change < params.in_out_params_indoor.resolution_X * 0.5) { - //std::cout << "skipping update_rgd_hierarchy due to small translation change" << std::endl; + // std::cout << "skipping update_rgd_hierarchy due to small translation change" << std::endl; return true; } Eigen::Affine3d m_rot = worker_data.intermediate_trajectory[0].inverse() * worker_data.intermediate_trajectory.back(); - + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_rot); - //std::cout << "m_rot: " << pose.om * 180.0 / M_PI << ", " << pose.fi * 180.0 / M_PI << ", " << pose.ka * 180.0 / M_PI - // << std::endl; + // std::cout << "m_rot: " << pose.om * 180.0 / M_PI << ", " << pose.fi * 180.0 / M_PI << ", " << pose.ka * 180.0 / M_PI + // << std::endl; - if (fabs(pose.om * 180.0 / M_PI) > 5.0 || fabs(pose.fi * 180.0 / M_PI) > 5.0 || fabs(pose.ka * 180.0 / M_PI) > 5.0){ - //std::cout << "skipping update_rgd_hierarchy due to large rotation change" << std::endl; - return true; + if (fabs(pose.om * 180.0 / M_PI) > 5.0 || fabs(pose.fi * 180.0 / M_PI) > 5.0 || fabs(pose.ka * 180.0 / M_PI) > 5.0) + { + // std::cout << "skipping update_rgd_hierarchy due to large rotation change" << std::endl; + return true; } update_rgd_hierarchy( diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp index 00f5e39a..b5c63842 100644 --- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp +++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp @@ -30,7 +30,6 @@ #include -#define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; const uint32_t window_width = 800; diff --git a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp index ab54ee17..7fbc53ca 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -46,7 +46,6 @@ std::vector infoLines = { "This program is optional step in MANDEYE // App specific shortcuts (using empty dummy until needed) std::vector appShortcuts(80, { "", "", "" }); -#define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; ImVec4 pc_color = ImVec4(1.0f, 0.0f, 0.0f, 1.00f); diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index d72b409b..aee200bd 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1104,16 +1104,31 @@ void loadFiles(std::vector input_file_names) // VQF initialization double avg_dt = SAMPLE_PERIOD; - /*if (imu_data.size() >= 2) + if (imu_data.size() >= 2) { - double t0 = std::get<0>(imu_data.front()).first; - double t1 = std::get<0>(imu_data.back()).first; + const double t0 = std::get<0>(imu_data.front()).first; + const double t1 = std::get<0>(imu_data.back()).first; + const double duration = t1 - t0; + if (t1 > t0) - avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); - }*/ + avg_dt = duration / static_cast(imu_data.size() - 1); + + if (duration > 24 * 60 * 60) + { + spdlog::error("Session is absurdly long : start time : {}, end time {}", t0, t1); + spdlog::error("Setting rate to {}", SAMPLE_PERIOD); + avg_dt = SAMPLE_PERIOD; + } + } VQFParams vqf_params; vqf_params.tauAcc = vqf_tauAcc > 0.0 ? vqf_tauAcc : 3.0; + // produce warning + if (std::fabs(avg_dt - SAMPLE_PERIOD) > 0.01) + { + spdlog::warn("The dt found is strange : {}", avg_dt); + } + spdlog::info("avg_dt: {}", avg_dt); VQF vqf(vqf_params, avg_dt); std::map> trajectory; diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp index fa339884..508785b6 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -126,7 +126,6 @@ static const std::vector appShortcuts = { { "Normal keys", "A", " { "", "Ctrl + right click", "" }, { "", "Ctrl + middle click", "" } }; -#define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; ImVec4 pc_neigbouring_color = ImVec4(0.5f, 0.5f, 0.5f, 1.0f); diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index aac3a549..24901071 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -134,7 +134,7 @@ namespace imu_utils static_cast(raw_imu_data[k].accelerometers.z()) }; FusionAhrsUpdateNoMagnetometer(&fusion_ahrs, gyroscope, accelerometer, static_cast(dt)); - + FusionQuaternion quat = FusionAhrsGetQuaternion(&fusion_ahrs); Eigen::Quaterniond q(quat.element.w, quat.element.x, quat.element.y, quat.element.z); orientations.push_back(q.toRotationMatrix());