Add IMU dt sanity checks before VQF init

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<std::string> infoLines = {

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<double>(imu_data.size() - 1);
+                avg_dt = duration / static_cast<double>(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);

apps/lidar_odometry_step_1/lidar_odometry.h

@@ -12,8 +12,6 @@
 #include <Core/export_laz.h>
 #include <Core/session.h>
 
-// #define SAMPLE_PERIOD (1.0 / 200.0)
-
 using Trajectory = std::map<double, std::tuple<Eigen::Matrix4d, double, RawIMUData>>;
 using Imu = std::vector<std::tuple<std::pair<double, double>, Eigen::Vector3f, Eigen::Vector3f>>;
 

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<std::string> infoLines = {

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(

apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp

@@ -30,7 +30,6 @@
 
 #include <spdlog/spdlog.h>
 
-#define SAMPLE_PERIOD (1.0 / 200.0)
 namespace fs = std::filesystem;
 
 const uint32_t window_width = 800;

apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp

@@ -46,7 +46,6 @@ std::vector<std::string> infoLines = { "This program is optional step in MANDEYE
 // App specific shortcuts (using empty dummy until needed)
 std::vector<ShortcutEntry> 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);

apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp

@@ -1104,16 +1104,31 @@ void loadFiles(std::vector<std::string> 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<double>(imu_data.size() - 1);
-        }*/
+                avg_dt = duration / static_cast<double>(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<double, std::pair<Eigen::Matrix4d, double>> trajectory;

apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp

@@ -126,7 +126,6 @@ static const std::vector<ShortcutEntry> 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);

core/src/imu_preintegration.cpp

@@ -134,7 +134,7 @@ namespace imu_utils
                                                      static_cast<float>(raw_imu_data[k].accelerometers.z()) };
 
                 FusionAhrsUpdateNoMagnetometer(&fusion_ahrs, gyroscope, accelerometer, static_cast<float>(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());