Added imu preintegration and migrated from Madgwick to VQF algorithm for orientation estimation

.gitmodules

@@ -53,3 +53,6 @@
 [submodule "3rdparty/UTL"]
 	path = 3rdparty/UTL
 	url = https://github.com/DmitriBogdanov/UTL.git
+[submodule "3rdparty/vqf"]
+	path = 3rdparty/vqf
+	url = https://github.com/dlaidig/vqf.git

CMakeLists.txt

@@ -434,7 +434,7 @@ else()
 endif()
 
 
-add_subdirectory(${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
+add_subdirectory(${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
 
 add_subdirectory(core)
 add_subdirectory(core_hd_mapping)

apps/compare_trajectories/CMakeLists.txt

@@ -25,11 +25,11 @@ target_include_directories(
   ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
   ${LASZIP_INCLUDE_DIR}/LASzip/include
   ${THIRDPARTY_DIRECTORY}/observation_equations/codes
-  ${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
+  ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
 
 target_link_libraries(
   mandeye_compare_trajectories
-  PRIVATE Fusion
+  PRIVATE vqf
   unordered_dense::unordered_dense
   spdlog::spdlog
   ${FREEGLUT_LIBRARY}

apps/concatenate_multi_livox/CMakeLists.txt

@@ -20,7 +20,7 @@ target_include_directories(
           ${LASZIP_INCLUDE_DIR}/LASzip/include
           ${THIRDPARTY_DIRECTORY}/glew-cmake/include
           ${THIRDPARTY_DIRECTORY}/observation_equations/codes
-          ${THIRDPARTY_DIRECTORY}/Fusion/Fusion
+          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp
           ${FREEGLUT_INCLUDE_DIR})
 
 target_link_libraries(concatenate_multi_livox PRIVATE core

apps/concatenate_multi_livox/concatenate_multi_livox.cpp

@@ -139,8 +139,8 @@ int main(int argc, char* argv[])
         for (const auto& [timestamp, gyro, accel] : imu)
         {
             out << static_cast<uint64_t>(1e9 * timestamp.first) << " " << static_cast<uint64_t>(1e9 * timestamp.second) << " "
-                << accel.axis.x << " " << accel.axis.y << " " << accel.axis.z << " " << gyro.axis.x << " " << gyro.axis.y << " "
-                << gyro.axis.z << "\n";
+                << accel.x() << " " << accel.y() << " " << accel.z() << " " << gyro.x() << " " << gyro.y() << " "
+                << gyro.z() << "\n";
         }
         std::cout << "Saved IMU data to: " << output_path_csv.string() << std::endl;
     }

apps/lidar_odometry_step_1/CMakeLists.txt

@@ -42,11 +42,11 @@ target_include_directories(
           ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
           ${LASZIP_INCLUDE_DIR}/LASzip/include
           ${THIRDPARTY_DIRECTORY}/observation_equations/codes
-          ${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
+          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
 
 target_link_libraries(
           lidar_odometry_step_1
-          PRIVATE Fusion
+          PRIVATE vqf
           unordered_dense::unordered_dense
           spdlog::spdlog
           UTL::include
@@ -99,11 +99,11 @@ target_include_directories(
           ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
           ${LASZIP_INCLUDE_DIR}/LASzip/include
           ${THIRDPARTY_DIRECTORY}/observation_equations/codes
-          ${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
+          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
 
 target_link_libraries(
           drag_folder_with_mandeye_data_and_drop_here-precision_forestry
-          PRIVATE Fusion
+          PRIVATE vqf
           unordered_dense::unordered_dense
           spdlog::spdlog
           UTL::include

apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp

@@ -446,12 +446,8 @@ void step1(const std::atomic<bool>& loPause, std::string input_folder_name)
             calculate_trajectory(
                 trajectory,
                 imu_data,
-                params.fusionConventionNwu,
-                params.fusionConventionEnu,
-                params.fusionConventionNed,
-                params.ahrs_gain,
-                full_debug_messages,
-                params.use_removie_imu_bias_from_first_stationary_scan);
+                params,
+                full_debug_messages);
             compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause);
             step_1_done = true;
         }
@@ -730,7 +726,7 @@ void settings_gui()
             if (fusionConvention < 0 || fusionConvention > 2)
                 fusionConvention = 0;
 
-            ImGui::Text("Fusion convention: ");
+            ImGui::Text("AHRS convention: ");
             if (ImGui::IsItemHovered())
                 ImGui::SetTooltip(
                     "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame");
@@ -756,18 +752,112 @@ void settings_gui()
             ImGui::NewLine();
 
             ImGui::Checkbox("Use motion from previous step", &params.use_motion_from_previous_step);
-            ImGui::InputDouble("AHRS gain", &params.ahrs_gain, 0.0, 0.0, "%.3f");
+            ImGui::InputDouble("VQF tauAcc [s]", &params.vqf_tauAcc, 0.0, 0.0, "%.3f");
             if (ImGui::IsItemHovered())
             {
                 ImGui::BeginTooltip();
-                ImGui::Text("Attitude and Heading Reference System gain:");
-                ImGui::Text(
-                    "How strongly the accelerometer/magnetometer corrections influence the orientation estimate versus gyroscope "
-                    "integration");
-                ImGui::Text("Larger value means faster response to changes in orientation, but more noise");
+                ImGui::Text("VQF accelerometer time constant (tauAcc) in seconds.");
+                ImGui::Text("Controls how strongly accelerometer corrects the gyroscope-based orientation.");
+                ImGui::Text("Higher = more gyro trust (stable but may drift). Lower = more acc trust (noisy but no drift).");
                 ImGui::EndTooltip();
             }
 
+            if (ImGui::TreeNode("VQF Gyro Bias Estimation"))
+            {
+                ImGui::Checkbox("Motion bias estimation", &params.vqf_motionBiasEstEnabled);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Enables gyroscope bias estimation during motion phases,\nbased on the inclination correction only (without magnetometer).");
+                if (params.vqf_motionBiasEstEnabled)
+                {
+                    ImGui::InputDouble("Bias sigma motion [deg/s]", &params.vqf_biasSigmaMotion, 0.0, 0.0, "%.4f");
+                    if (ImGui::IsItemHovered())
+                        ImGui::SetTooltip("Std dev of converged bias estimation uncertainty during motion.\nDetermines trust on motion bias estimation updates.\nSmall value leads to fast convergence. Default: 0.1");
+                    ImGui::InputDouble("Bias vertical forgetting", &params.vqf_biasVerticalForgettingFactor, 0.0, 0.0, "%.6f");
+                    if (ImGui::IsItemHovered())
+                        ImGui::SetTooltip("Forgetting factor for unobservable bias in vertical direction during motion.\nGyro bias is not observable vertically without magnetometer.\nRelative weight of artificial zero measurement ensuring\nbias estimate decays to zero. Default: 0.0001");
+                }
+                ImGui::Checkbox("Rest bias estimation", &params.vqf_restBiasEstEnabled);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Enables rest detection and gyroscope bias estimation during rest phases.\nDuring rest, gyro bias is estimated from low-pass filtered gyro readings.");
+                if (params.vqf_restBiasEstEnabled)
+                {
+                    ImGui::InputDouble("Bias sigma rest [deg/s]", &params.vqf_biasSigmaRest, 0.0, 0.0, "%.4f");
+                    if (ImGui::IsItemHovered())
+                        ImGui::SetTooltip("Std dev of converged bias estimation uncertainty during rest.\nDetermines trust on rest bias estimation updates.\nSmall value leads to fast convergence. Default: 0.03");
+                }
+                ImGui::InputDouble("Bias sigma init [deg/s]", &params.vqf_biasSigmaInit, 0.0, 0.0, "%.3f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Std dev of the initial bias estimation uncertainty. Default: 0.5 deg/s");
+                ImGui::InputDouble("Bias forgetting time [s]", &params.vqf_biasForgettingTime, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Time in which bias estimation uncertainty increases from 0 to 0.1 deg/s.\nDetermines the system noise assumed by the Kalman filter. Default: 100.0");
+                ImGui::InputDouble("Bias clip [deg/s]", &params.vqf_biasClip, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Maximum expected gyroscope bias.\nUsed to clip bias estimate and measurement error in update step.\nAlso used by rest detection to not regard large constant angular rate as rest.\nDefault: 2.0");
+                ImGui::TreePop();
+            }
+
+            if (params.vqf_restBiasEstEnabled && ImGui::TreeNode("VQF Rest Detection"))
+            {
+                ImGui::InputDouble("Rest min time [s]", &params.vqf_restMinT, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Time threshold for rest detection.\nRest is detected when measurements have been close to\nthe low-pass filtered reference for the given time. Default: 1.5");
+                ImGui::InputDouble("Rest filter tau [s]", &params.vqf_restFilterTau, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Time constant for the second-order Butterworth low-pass filter\nused to obtain the reference for rest detection. Default: 0.5");
+                ImGui::InputDouble("Rest threshold gyro [deg/s]", &params.vqf_restThGyr, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Angular velocity threshold for rest detection.\nDeviation norm between measurement and reference must be below threshold.\nEach component must also be below biasClip. Default: 2.0");
+                ImGui::InputDouble("Rest threshold acc [m/s2]", &params.vqf_restThAcc, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Acceleration threshold for rest detection.\nDeviation norm between measurement and reference must be below threshold.\nDefault: 0.5");
+                ImGui::TreePop();
+            }
+
+            ImGui::Checkbox("Use magnetometer", &params.vqf_useMagnetometer);
+            if (ImGui::IsItemHovered())
+                ImGui::SetTooltip("Enable 9D mode (gyro+acc+mag) for absolute heading correction.\nDefault: off (6D mode, gyro+acc only, heading from gyro integration).");
+            if (params.vqf_useMagnetometer && ImGui::TreeNode("VQF Magnetometer"))
+            {
+                ImGui::InputDouble("tauMag [s]", &params.vqf_tauMag, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Time constant for magnetometer update.\nSmall values imply trust on magnetometer, large values trust on gyroscope.\nCorresponds to cutoff frequency of first-order LP filter\nfor heading correction. Default: 9.0");
+                ImGui::Checkbox("Mag disturbance rejection", &params.vqf_magDistRejectionEnabled);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Enables magnetic disturbance detection and rejection.\nFor short disturbances, mag correction is fully disabled.\nFor long disturbances (>magMaxRejectionTime), correction uses\nincreased time constant (magRejectionFactor).");
+                ImGui::InputDouble("Mag current tau [s]", &params.vqf_magCurrentTau, 0.0, 0.0, "%.3f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Time constant for current norm/dip value in disturbance detection.\nFast LP filter for robustness with noisy or async mag measurements.\nSet to -1 to disable. Default: 0.05");
+                ImGui::InputDouble("Mag ref tau [s]", &params.vqf_magRefTau, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Time constant for adjustment of the magnetic field reference.\nAllows reference to converge to observed undisturbed field. Default: 20.0");
+                ImGui::InputDouble("Mag norm threshold", &params.vqf_magNormTh, 0.0, 0.0, "%.3f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Relative threshold for magnetic field strength for disturbance detection.\nRelative to the reference norm. Default: 0.1 (10%%)");
+                ImGui::InputDouble("Mag dip threshold [deg]", &params.vqf_magDipTh, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Threshold for the magnetic field dip angle for disturbance detection. Default: 10.0");
+                ImGui::InputDouble("Mag new time [s]", &params.vqf_magNewTime, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Duration after which to accept a different homogeneous magnetic field.\nNew reference accepted when within magNormTh and magDipTh for this time.\nOnly phases with sufficient movement (magNewMinGyr) count. Default: 20.0");
+                ImGui::InputDouble("Mag new first time [s]", &params.vqf_magNewFirstTime, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Duration after which to accept a homogeneous magnetic field for the first time.\nUsed instead of magNewTime when no current estimate exists,\nto allow faster initial reference acquisition. Default: 5.0");
+                ImGui::InputDouble("Mag new min gyro [deg/s]", &params.vqf_magNewMinGyr, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Minimum angular velocity needed to count time for new mag field acceptance.\nPeriods with angular velocity norm below this threshold\ndo not count towards magNewTime. Default: 20.0");
+                ImGui::InputDouble("Mag min undisturbed [s]", &params.vqf_magMinUndisturbedTime, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Minimum duration within thresholds after which to regard\nthe field as undisturbed again. Default: 0.5");
+                ImGui::InputDouble("Mag max rejection [s]", &params.vqf_magMaxRejectionTime, 0.0, 0.0, "%.1f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Maximum duration of full magnetic disturbance rejection.\nUp to this duration, heading correction is fully disabled\nand tracked by gyroscope only. After this, correction uses\nincreased time constant (magRejectionFactor). Default: 60.0");
+                ImGui::InputDouble("Mag rejection factor", &params.vqf_magRejectionFactor, 0.0, 0.0, "%.2f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Factor by which to slow heading correction during long disturbed phases.\nAfter magMaxRejectionTime of full rejection, correction uses\nthis factor to increase the time constant. Default: 2.0");
+                ImGui::TreePop();
+            }
+
             ImGui::PopItemWidth();
         }
         ImGui::NewLine();
@@ -1435,12 +1525,8 @@ void step1(
         calculate_trajectory(
             trajectory,
             imu_data,
-            params.fusionConventionNwu,
-            params.fusionConventionEnu,
-            params.fusionConventionNed,
-            params.ahrs_gain,
-            full_debug_messages,
-            params.use_removie_imu_bias_from_first_stationary_scan);
+            params,
+            full_debug_messages);
         compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause);
         std::cout << "step_1_done" << std::endl;
     }