diff --git a/.gitmodules b/.gitmodules index da3afb9d..cb81a74b 100644 --- a/.gitmodules +++ b/.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 diff --git a/3rdparty/vqf b/3rdparty/vqf new file mode 160000 index 00000000..b66ff218 --- /dev/null +++ b/3rdparty/vqf @@ -0,0 +1 @@ +Subproject commit b66ff218128ab0993b4ac19b538a4a57863c3382 diff --git a/CMakeLists.txt b/CMakeLists.txt index a77873c9..e7a5962d 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/apps/compare_trajectories/CMakeLists.txt b/apps/compare_trajectories/CMakeLists.txt index afc1b698..862882bf 100644 --- a/apps/compare_trajectories/CMakeLists.txt +++ b/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} diff --git a/apps/concatenate_multi_livox/CMakeLists.txt b/apps/concatenate_multi_livox/CMakeLists.txt index ce887016..644a9b62 100644 --- a/apps/concatenate_multi_livox/CMakeLists.txt +++ b/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 diff --git a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp index 7022a5ef..d059c1d5 100644 --- a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp +++ b/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(1e9 * timestamp.first) << " " << static_cast(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; } diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 341a7a3f..68ac48c0 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/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 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 0e2608bb..f396d0c6 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 @@ -446,12 +446,8 @@ void step1(const std::atomic& 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", ¶ms.use_motion_from_previous_step); - ImGui::InputDouble("AHRS gain", ¶ms.ahrs_gain, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("VQF tauAcc [s]", ¶ms.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", ¶ms.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]", ¶ms.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", ¶ms.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", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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", ¶ms.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]", ¶ms.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", ¶ms.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]", ¶ms.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]", ¶ms.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", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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", ¶ms.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; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index acc02811..2917fd17 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -7,8 +7,6 @@ namespace fs = std::filesystem; -const float RAD_TO_DEG = 180.0f / static_cast(M_PI); - bool load_data( std::vector& input_file_names, LidarOdometryParams& params, @@ -207,7 +205,7 @@ bool load_data( std::size_t minSize = std::min(laz_files.size(), csv_files.size()); // Each thread loads into its own local vector - std::vector, FusionVector, FusionVector>>> imuChunks(minSize); + std::vector imuChunks(minSize); tbb::parallel_for( tbb::blocked_range(0, minSize), @@ -319,115 +317,71 @@ bool load_data( void calculate_trajectory( Trajectory& trajectory, Imu& imu_data, - bool fusionConventionNwu, - bool fusionConventionEnu, - bool fusionConventionNed, - double ahrs_gain, - bool debugMsg, - bool use_remove_imu_bias_from_first_stationary_scan) + const LidarOdometryParams& params, + bool debugMsg) { UTL_PROFILER_SCOPE("calculate_trajectory"); - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - ahrs.settings.convention = FusionConventionNwu; - else if (fusionConventionEnu) - ahrs.settings.convention = FusionConventionEnu; - else if (fusionConventionNed) - ahrs.settings.convention = FusionConventionNed; - ahrs.settings.gain = ahrs_gain; - int counter = 1; + int counter = 1; double previous_time_stamp = 0.0; - static bool first = true; - static double last_ts; - std::cout << "start calculating trajectory.." << std::endl; - double bias_gyr_x = 0.0; - double bias_gyr_y = 0.0; - double bias_gyr_z = 0.0; - - if (use_remove_imu_bias_from_first_stationary_scan) + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; + if (imu_data.size() >= 2) { - if (imu_data.size() > 1000) - { - std::vector gyr_x; - std::vector gyr_y; - std::vector gyr_z; - - for (int i = 0; i < 1000; i++) - { - const auto& [timestamp_pair, gyr, acc] = imu_data[i]; - - gyr_x.push_back(gyr.axis.x * RAD_TO_DEG); - gyr_y.push_back(gyr.axis.y * RAD_TO_DEG); - gyr_z.push_back(gyr.axis.z * RAD_TO_DEG); - } - - // Median without full sort - std::nth_element(gyr_x.begin(), gyr_x.begin() + 500, gyr_x.end()); - std::nth_element(gyr_y.begin(), gyr_y.begin() + 500, gyr_y.end()); - std::nth_element(gyr_z.begin(), gyr_z.begin() + 500, gyr_z.end()); - - bias_gyr_x = gyr_x[500]; - bias_gyr_y = gyr_y[500]; - bias_gyr_z = gyr_z[500]; - } - - std::cout << "------------------" << std::endl; - std::cout << "GYRO BIAS" << std::endl; - std::cout << "bias_gyr_x [deg/s]: " << bias_gyr_x << std::endl; - std::cout << "bias_gyr_y [deg/s]: " << bias_gyr_y << std::endl; - std::cout << "bias_gyr_z [deg/s]: " << bias_gyr_z << std::endl; - std::cout << "------------------" << std::endl; + double t0 = std::get<0>(imu_data.front()).first; + double t1 = std::get<0>(imu_data.back()).first; + if (t1 > t0) + avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); } + VQFParams vqf_params = buildVQFParams(params); + VQF vqf(vqf_params, avg_dt); + std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc + << ", useMag=" << (params.vqf_useMagnetometer ? "true" : "false") + << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl; + for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * RAD_TO_DEG) - static_cast(bias_gyr_x), - static_cast(gyr.axis.y * RAD_TO_DEG) - static_cast(bias_gyr_y), - static_cast(gyr.axis.z * RAD_TO_DEG) - static_cast(bias_gyr_z) }; - const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); - first = false; - // last_ts = timestamp_pair.first; - } + // IMU data: gyro in rad/s, acc in g + // VQF expects: gyro in rad/s, acc in m/s² + const double g = 9.81; + vqf_real_t gyr_vqf[3] = { + static_cast(gyr.x()), + static_cast(gyr.y()), + static_cast(gyr.z()) }; + vqf_real_t acc_vqf[3] = { + static_cast(acc.x()) * g, + static_cast(acc.y()) * g, + static_cast(acc.z()) * g }; + + vqf.update(gyr_vqf, acc_vqf); + + vqf_real_t quat[4]; + if (params.vqf_useMagnetometer) + vqf.getQuat9D(quat); else - { - double curr_ts = timestamp_pair.first; - - double ts_diff = curr_ts - last_ts; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - - last_ts = timestamp_pair.first; - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + vqf.getQuat6D(quat); + Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]); Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; t.rotate(d); + // Store gyro/acc in original units for RawIMUData RawIMUData rawImuData{ timestamp_pair.first, - { accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z }, - { gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z } }; // check timestamp + { static_cast(acc.x()), static_cast(acc.y()), static_cast(acc.z()) }, + { static_cast(gyr.x()), static_cast(gyr.y()), static_cast(gyr.z()) } }; trajectory[timestamp_pair.first] = std::make_tuple(t.matrix(), timestamp_pair.second, rawImuData); if (debugMsg) { - const FusionEuler euler = FusionQuaternionToEuler(quat); counter++; if (counter % 100 == 0) { - std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw << " [" - << counter++ << " of " << imu_data.size() << "]" << std::endl; + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); + std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" + << counter << " of " << imu_data.size() << "]" << std::endl; } } @@ -1068,12 +1022,8 @@ std::vector run_lidar_odometry(const std::string& input_dir, LidarOd calculate_trajectory( trajectory, imu_data, - params.fusionConventionNwu, - params.fusionConventionEnu, - params.fusionConventionNed, - params.ahrs_gain, - true, - params.use_removie_imu_bias_from_first_stationary_scan); + params, + true); std::atomic pause{ false }; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index e080ecb8..a5ee1a27 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -15,7 +15,7 @@ // #define SAMPLE_PERIOD (1.0 / 200.0) using Trajectory = std::map>; -using Imu = std::vector, FusionVector, FusionVector>>; +using Imu = std::vector, Eigen::Vector3f, Eigen::Vector3f>>; bool load_data( std::vector& input_file_names, @@ -26,12 +26,8 @@ bool load_data( void calculate_trajectory( Trajectory& trajectory, Imu& imu_data, - bool fusionConventionNwu, - bool fusionConventionEnu, - bool fusionConventionNed, - double ahrs_gain, - bool debugMsg, - bool use_removie_imu_bias_from_first_stationary_scan); + const LidarOdometryParams& params, + bool debugMsg); bool compute_step_1( const std::vector>& pointsPerFile, LidarOdometryParams& params, diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 363735ab..965d3add 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -465,7 +465,7 @@ void alternative_approach() } std::cout << "loading imu" << std::endl; - std::vector, FusionVector, FusionVector>> imu_data; + Imu imu_data; std::for_each(std::begin(csv_files), std::end(csv_files), [&imu_data](const std::string& fn) { @@ -473,67 +473,52 @@ void alternative_approach() std::cout << fn << std::endl; imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); }); - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (params.fusionConventionNwu) - { - ahrs.settings.convention = FusionConventionNwu; - } - if (params.fusionConventionEnu) - { - ahrs.settings.convention = FusionConventionEnu; - } - if (params.fusionConventionNed) + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; + if (imu_data.size() >= 2) { - ahrs.settings.convention = FusionConventionNed; + double t0 = std::get<0>(imu_data.front()).first; + double t1 = std::get<0>(imu_data.back()).first; + if (t1 > t0) + avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); } + VQFParams vqf_params = buildVQFParams(params); + VQF vqf(vqf_params, avg_dt); + std::map trajectory; int counter = 1; - static bool first = true; - static double last_ts; for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * RAD_TO_DEG), static_cast(gyr.axis.y * RAD_TO_DEG), static_cast(gyr.axis.z * RAD_TO_DEG) }; - const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - - //FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); - first = false; - } + const double g = 9.81; + vqf_real_t gyr_vqf[3] = { + static_cast(gyr.x()), + static_cast(gyr.y()), + static_cast(gyr.z()) }; + vqf_real_t acc_vqf[3] = { + static_cast(acc.x()) * g, + static_cast(acc.y()) * g, + static_cast(acc.z()) * g }; + + vqf.update(gyr_vqf, acc_vqf); + + vqf_real_t quat[4]; + if (params.vqf_useMagnetometer) + vqf.getQuat9D(quat); else - { - double curr_ts = timestamp_pair.first; - double ts_diff = curr_ts - last_ts; - if (ts_diff < 0.01) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - else - { - std::cout << "IMU TS jump!!!" << std::endl; - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); - } - } - last_ts = timestamp_pair.first; - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + vqf.getQuat6D(quat); + Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]); Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; t.rotate(d); trajectory[timestamp_pair.first] = t.matrix(); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); counter++; if (counter % 100 == 0) { - std::cout << << "Roll " << euler.angle.roll<< ", Pitch " << euler.angle.pitch<< ", Yaw " << euler.angle.yaw<< " [" << counter++ << " of " << imu_data.size() << "]"<< std::endl; + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); + std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl; } } @@ -626,12 +611,8 @@ void step1(const std::atomic& loPause) 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; } @@ -910,7 +891,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"); @@ -936,18 +917,132 @@ void settings_gui() ImGui::NewLine(); ImGui::Checkbox("Use motion from previous step", ¶ms.use_motion_from_previous_step); - ImGui::InputDouble("AHRS gain", ¶ms.ahrs_gain, 0.0, 0.0, "%.3f"); + ImGui::Checkbox("Use IMU preintegration", ¶ms.use_imu_preintegration); + if (params.use_imu_preintegration) + { + const char* methods[] = { + "Euler, no gravity compensation", + "Trapezoidal, no gravity compensation", + "Euler, gravity comp. (initial trajectory orientations)", + "Trapezoidal, gravity comp. (initial trajectory orientations)", + "Kalman, gravity comp. (initial trajectory orientations)", + "Euler, gravity comp. (per-worker VQF orientations)", + "Trapezoidal, gravity comp. (per-worker VQF orientations)", + "Kalman, gravity comp. (per-worker VQF orientations)" }; + ImGui::Combo("IMU preintegration method", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); + } + ImGui::InputDouble("VQF tauAcc [s]", ¶ms.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", ¶ms.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]", ¶ms.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", ¶ms.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", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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", ¶ms.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]", ¶ms.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", ¶ms.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]", ¶ms.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]", ¶ms.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", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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]", ¶ms.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", ¶ms.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(); @@ -1647,12 +1742,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; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index b95cc2ba..9690faf7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -346,9 +346,9 @@ bool save_poses(const std::string& file_name, const std::vector return true; } -std::vector, FusionVector, FusionVector>> load_imu(const std::string& imu_file, int imuToUse) +std::vector, Eigen::Vector3f, Eigen::Vector3f>> load_imu(const std::string& imu_file, int imuToUse) { - std::vector, FusionVector, FusionVector>> all_data; + std::vector, Eigen::Vector3f, Eigen::Vector3f>> all_data; csv::CSVFormat format; format.delimiter({ ' ', ',', '\t' }); @@ -396,14 +396,14 @@ std::vector, FusionVector, FusionVector>> l { double timestamp = row["timestamp"].get(); double timestampUnix = row["timestampUnix"].get(); - FusionVector gyr; - gyr.axis.x = row["gyroX"].get(); - gyr.axis.y = row["gyroY"].get(); - gyr.axis.z = row["gyroZ"].get(); - FusionVector acc; - acc.axis.x = row["accX"].get(); - acc.axis.y = row["accY"].get(); - acc.axis.z = row["accZ"].get(); + Eigen::Vector3f gyr( + row["gyroX"].get(), + row["gyroY"].get(), + row["gyroZ"].get()); + Eigen::Vector3f acc( + row["accX"].get(), + row["accY"].get(), + row["accZ"].get()); all_data.emplace_back(std::pair(timestamp / 1e9, timestampUnix / 1e9), gyr, acc); } } @@ -433,15 +433,14 @@ std::vector, FusionVector, FusionVector>> l if (data[0] > 0 && imuId == imuToUse) { - FusionVector gyr; - gyr.axis.x = data[1]; - gyr.axis.y = data[2]; - gyr.axis.z = data[3]; - - FusionVector acc; - acc.axis.x = data[4]; - acc.axis.y = data[5]; - acc.axis.z = data[6]; + Eigen::Vector3f gyr( + static_cast(data[1]), + static_cast(data[2]), + static_cast(data[3])); + Eigen::Vector3f acc( + static_cast(data[4]), + static_cast(data[5]), + static_cast(data[6])); all_data.emplace_back(std::pair(data[0] / 1e9, timestampUnix / 1e9), gyr, acc); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 34a3ae72..18567a21 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include #include @@ -65,11 +65,44 @@ struct LidarOdometryParams double threshould_output_filter = 0.5; // for export --> all points xyz.norm() < threshould_output_filter will be removed int min_counter_concatenated_trajectory_nodes = 10; // for export - // Madgwick filter + // AHRS (VQF) — fusionConvention params kept for TOML backwards compatibility bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; - double ahrs_gain = 0.5; + + // VQF core + double vqf_tauAcc = 0.5; // accelerometer time constant [s] (higher = more gyro trust) + + // VQF gyroscope bias estimation + bool vqf_motionBiasEstEnabled = true; // estimate gyro bias during motion + bool vqf_restBiasEstEnabled = true; // estimate gyro bias during rest + double vqf_biasSigmaInit = 0.5; // initial bias uncertainty [°/s] + double vqf_biasForgettingTime = 100.0; // time for uncertainty to grow 0→0.1 °/s [s] + double vqf_biasClip = 2.0; // max expected gyro bias [°/s] + double vqf_biasSigmaMotion = 0.1; // converged bias uncertainty during motion [°/s] + double vqf_biasVerticalForgettingFactor = 0.0001; // forgetting for unobservable vertical bias + double vqf_biasSigmaRest = 0.03; // converged bias uncertainty during rest [°/s] + + // VQF rest detection + double vqf_restMinT = 1.5; // time threshold for rest detection [s] + double vqf_restFilterTau = 0.5; // LP filter time constant for rest detection [s] + double vqf_restThGyr = 2.0; // gyro threshold for rest detection [°/s] + double vqf_restThAcc = 0.5; // acc threshold for rest detection [m/s²] + + // VQF magnetometer (only used when vqf_useMagnetometer is true) + bool vqf_useMagnetometer = false; // use 9D mode (with magnetometer) instead of 6D + double vqf_tauMag = 9.0; // magnetometer time constant [s] + bool vqf_magDistRejectionEnabled = true; // magnetic disturbance detection & rejection + double vqf_magCurrentTau = 0.05; // LP filter for current mag norm/dip [s] + double vqf_magRefTau = 20.0; // adjustment time for mag reference [s] + double vqf_magNormTh = 0.1; // relative threshold for mag field strength + double vqf_magDipTh = 10.0; // threshold for mag dip angle [°] + double vqf_magNewTime = 20.0; // time to accept new mag field [s] + double vqf_magNewFirstTime = 5.0; // time to accept first mag field [s] + double vqf_magNewMinGyr = 20.0; // min angular velocity for mag acceptance [°/s] + double vqf_magMinUndisturbedTime = 0.5; // min undisturbed time [s] + double vqf_magMaxRejectionTime = 60.0; // max full mag rejection duration [s] + double vqf_magRejectionFactor = 2.0; // slowdown factor for heading correction // lidar odometry control bool use_motion_from_previous_step = true; @@ -154,6 +187,10 @@ struct LidarOdometryParams bool use_removie_imu_bias_from_first_stationary_scan = false; + // IMU preintegration + bool use_imu_preintegration = true; + int imu_preintegration_method = 6; // 0=euler_body, 1=trapezoidal_body, 2=euler_gravity, 3=trapezoidal_gravity, 4=kalman, 5=euler_ahrs, 6=trapezoidal_ahrs, 7=kalman_ahrs + // ablation study bool ablation_study_use_planarity = false; bool ablation_study_use_norm = false; @@ -163,6 +200,41 @@ struct LidarOdometryParams bool save_index_pose = false; }; +inline VQFParams buildVQFParams(const LidarOdometryParams& p) +{ + VQFParams vp; + vp.tauAcc = p.vqf_tauAcc > 0.0 ? p.vqf_tauAcc : 3.0; + vp.tauMag = p.vqf_tauMag; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vp.motionBiasEstEnabled = p.vqf_motionBiasEstEnabled; +#endif + vp.restBiasEstEnabled = p.vqf_restBiasEstEnabled; + vp.magDistRejectionEnabled = p.vqf_magDistRejectionEnabled; + vp.biasSigmaInit = p.vqf_biasSigmaInit; + vp.biasForgettingTime = p.vqf_biasForgettingTime; + vp.biasClip = p.vqf_biasClip; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vp.biasSigmaMotion = p.vqf_biasSigmaMotion; + vp.biasVerticalForgettingFactor = p.vqf_biasVerticalForgettingFactor; +#endif + vp.biasSigmaRest = p.vqf_biasSigmaRest; + vp.restMinT = p.vqf_restMinT; + vp.restFilterTau = p.vqf_restFilterTau; + vp.restThGyr = p.vqf_restThGyr; + vp.restThAcc = p.vqf_restThAcc; + vp.magCurrentTau = p.vqf_magCurrentTau; + vp.magRefTau = p.vqf_magRefTau; + vp.magNormTh = p.vqf_magNormTh; + vp.magDipTh = p.vqf_magDipTh; + vp.magNewTime = p.vqf_magNewTime; + vp.magNewFirstTime = p.vqf_magNewFirstTime; + vp.magNewMinGyr = p.vqf_magNewMinGyr; + vp.magMinUndisturbedTime = p.vqf_magMinUndisturbedTime; + vp.magMaxRejectionTime = p.vqf_magMaxRejectionTime; + vp.magRejectionFactor = p.vqf_magRejectionFactor; + return vp; +} + // this function finds interpolated pose between two poses according to query_time Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time); @@ -208,7 +280,7 @@ void update_rgd_spherical_coordinates( //! @param imu_file - path to file with IMU data //! @param imuToUse - id number of IMU to use, the same index as in pointcloud return by @ref load_point_cloud //! @return vector of tuples (std::pair, angular_velocity, linear_acceleration) -std::vector, FusionVector, FusionVector>> load_imu(const std::string& imu_file, int imuToUse); +std::vector, Eigen::Vector3f, Eigen::Vector3f>> load_imu(const std::string& imu_file, int imuToUse); //! This function load point cloud from LAS/LAZ file. //! Optionally it can apply extrinsic calibration to each point. 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 bedb70e3..2775f691 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1,5 +1,6 @@ #include "lidar_odometry_utils.h" #include +#include #include #include #include @@ -2357,6 +2358,24 @@ bool process_worker_step_1( { UTL_PROFILER_SCOPE("process_worker_step_1"); + { + static int last_logged_method = -2; + int current_state = params.use_imu_preintegration ? params.imu_preintegration_method : -1; + if (current_state != last_logged_method) + { + if (params.use_imu_preintegration) + { + auto method = static_cast(params.imu_preintegration_method); + spdlog::info("IMU preintegration enabled with method: {}", to_string(method)); + } + else + { + spdlog::info("IMU preintegration disabled"); + } + last_logged_method = current_state; + } + } + Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); if (i > 1 && params.use_motion_from_previous_step) { @@ -2394,34 +2413,57 @@ bool process_worker_step_1( // 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); mean_shift /= // ((worker_data[i - 2].intermediate_trajectory.size()) - 2); - bool use_imu_preintegtation = false; - - if (use_imu_preintegtation) + if (params.use_imu_preintegration) { - // change mean_shift with preintegrated IMU data - // use rotation from std::vector new_trajectory; - // new_trajectory.size() == worker_data[i].raw_imu_data.size(); - // mean_shift = preintegrate_imu(worker_data[i].raw_imu_data); ToDo + IntegrationParams imu_params; + + // Estimate initial velocity fully SM-independent: + // - Direction: AHRS orientation (VQF, not SM-optimized) + // - Speed: from previous worker's MOTION MODEL displacement (IMU prediction, not SM result) + Eigen::Vector3d prev_mm_displacement = + prev_worker_data.intermediate_trajectory_motion_model.back().translation() - + prev_worker_data.intermediate_trajectory_motion_model.front().translation(); + + if (worker_data.raw_imu_data.size() >= 2) + { + double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; + if (total_imu_time > 0.0) + { + double speed = prev_mm_displacement.norm() / total_imu_time; + + // Use AHRS orientation from current worker (original VQF, not SM-optimized) + // to determine forward direction — breaks SM feedback loop + Eigen::Matrix3d R_ahrs = worker_data.intermediate_trajectory[0].linear(); + Eigen::Vector3d forward_global = R_ahrs * Eigen::Vector3d(1, 0, 0); + forward_global.z() = 0; // project to horizontal plane + if (forward_global.norm() > 1e-6) + imu_params.initial_velocity = forward_global.normalized() * speed; + else + imu_params.initial_velocity = Eigen::Vector3d::Zero(); + } + } + + mean_shift = ImuPreintegration::create_and_preintegrate( + static_cast(params.imu_preintegration_method), + worker_data.raw_imu_data, new_trajectory, imu_params); } else { mean_shift = prev_worker_data.intermediate_trajectory[prev_worker_data.intermediate_trajectory.size() - 1].translation() - prev_prev_worker_data.intermediate_trajectory[prev_prev_worker_data.intermediate_trajectory.size() - 1].translation(); - // mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - - // worker_data[i - 2].intermediate_trajectory[0].translation(); mean_shift /= (prev_worker_data.intermediate_trajectory.size()); + } - if (mean_shift.norm() > 1.0) - { - spdlog::warn("mean_shift.norm() > 1.0"); - mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); - } + if (mean_shift.norm() > 1.0) + { + spdlog::warn("mean_shift.norm() > 1.0, resetting to zero (was: {})", mean_shift.norm()); + mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); + } - for (int tr = 0; tr < new_trajectory.size(); tr++) - { - new_trajectory[tr].translation() += mean_shift * tr; - } + for (int tr = 0; tr < new_trajectory.size(); tr++) + { + new_trajectory[tr].translation() += mean_shift * tr; } worker_data.intermediate_trajectory = new_trajectory; diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index cf547736..30793c9e 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -37,7 +37,32 @@ class TomlIO { "fusionConventionNwu", &LidarOdometryParams::fusionConventionNwu }, { "fusionConventionEnu", &LidarOdometryParams::fusionConventionEnu }, { "fusionConventionNed", &LidarOdometryParams::fusionConventionNed }, - { "ahrs_gain", &LidarOdometryParams::ahrs_gain }, + { "vqf_tauAcc", &LidarOdometryParams::vqf_tauAcc }, + { "vqf_motionBiasEstEnabled", &LidarOdometryParams::vqf_motionBiasEstEnabled }, + { "vqf_restBiasEstEnabled", &LidarOdometryParams::vqf_restBiasEstEnabled }, + { "vqf_biasSigmaInit", &LidarOdometryParams::vqf_biasSigmaInit }, + { "vqf_biasForgettingTime", &LidarOdometryParams::vqf_biasForgettingTime }, + { "vqf_biasClip", &LidarOdometryParams::vqf_biasClip }, + { "vqf_biasSigmaMotion", &LidarOdometryParams::vqf_biasSigmaMotion }, + { "vqf_biasVerticalForgettingFactor", &LidarOdometryParams::vqf_biasVerticalForgettingFactor }, + { "vqf_biasSigmaRest", &LidarOdometryParams::vqf_biasSigmaRest }, + { "vqf_restMinT", &LidarOdometryParams::vqf_restMinT }, + { "vqf_restFilterTau", &LidarOdometryParams::vqf_restFilterTau }, + { "vqf_restThGyr", &LidarOdometryParams::vqf_restThGyr }, + { "vqf_restThAcc", &LidarOdometryParams::vqf_restThAcc }, + { "vqf_useMagnetometer", &LidarOdometryParams::vqf_useMagnetometer }, + { "vqf_tauMag", &LidarOdometryParams::vqf_tauMag }, + { "vqf_magDistRejectionEnabled", &LidarOdometryParams::vqf_magDistRejectionEnabled }, + { "vqf_magCurrentTau", &LidarOdometryParams::vqf_magCurrentTau }, + { "vqf_magRefTau", &LidarOdometryParams::vqf_magRefTau }, + { "vqf_magNormTh", &LidarOdometryParams::vqf_magNormTh }, + { "vqf_magDipTh", &LidarOdometryParams::vqf_magDipTh }, + { "vqf_magNewTime", &LidarOdometryParams::vqf_magNewTime }, + { "vqf_magNewFirstTime", &LidarOdometryParams::vqf_magNewFirstTime }, + { "vqf_magNewMinGyr", &LidarOdometryParams::vqf_magNewMinGyr }, + { "vqf_magMinUndisturbedTime", &LidarOdometryParams::vqf_magMinUndisturbedTime }, + { "vqf_magMaxRejectionTime", &LidarOdometryParams::vqf_magMaxRejectionTime }, + { "vqf_magRejectionFactor", &LidarOdometryParams::vqf_magRejectionFactor }, { "use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step }, { "nr_iter", &LidarOdometryParams::nr_iter }, { "sliding_window_trajectory_length_threshold", &LidarOdometryParams::sliding_window_trajectory_length_threshold }, @@ -82,7 +107,9 @@ class TomlIO { "rgd_sf_sigma_ka_deg", &LidarOdometryParams::rgd_sf_sigma_ka_deg }, { "max_distance_lidar_rigid_sf", &LidarOdometryParams::max_distance_lidar_rigid_sf }, { "current_output_dir", &LidarOdometryParams::current_output_dir }, - { "working_directory_preview", &LidarOdometryParams::working_directory_preview } + { "working_directory_preview", &LidarOdometryParams::working_directory_preview }, + { "use_imu_preintegration", &LidarOdometryParams::use_imu_preintegration }, + { "imu_preintegration_method", &LidarOdometryParams::imu_preintegration_method } }; // Special handling for TaitBryanPose members @@ -101,7 +128,18 @@ class TomlIO "decimation", "threshould_output_filter", "min_counter_concatenated_trajectory_nodes" } }, - { "madgwick_filter", { "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", "ahrs_gain" } }, + { "ahrs_vqf", { + "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", + "vqf_tauAcc", + "vqf_motionBiasEstEnabled", "vqf_restBiasEstEnabled", + "vqf_biasSigmaInit", "vqf_biasForgettingTime", "vqf_biasClip", + "vqf_biasSigmaMotion", "vqf_biasVerticalForgettingFactor", "vqf_biasSigmaRest", + "vqf_restMinT", "vqf_restFilterTau", "vqf_restThGyr", "vqf_restThAcc", + "vqf_useMagnetometer", "vqf_tauMag", "vqf_magDistRejectionEnabled", + "vqf_magCurrentTau", "vqf_magRefTau", "vqf_magNormTh", "vqf_magDipTh", + "vqf_magNewTime", "vqf_magNewFirstTime", "vqf_magNewMinGyr", + "vqf_magMinUndisturbedTime", "vqf_magMaxRejectionTime", "vqf_magRejectionFactor" + } }, { "lidar_odometry_control", { "use_motion_from_previous_step", "nr_iter", @@ -144,6 +182,7 @@ class TomlIO "rgd_sf_sigma_om_deg", "rgd_sf_sigma_fi_deg", "rgd_sf_sigma_ka_deg" } }, + { "imu_preintegration", { "use_imu_preintegration", "imu_preintegration_method" } }, { "paths", { "current_output_dir", "working_directory_preview" } }, { "misc", { "clear_color" } } }; diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt index ae2a6317..d8343f2b 100644 --- a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -26,13 +26,13 @@ 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_compile_definitions(livox_mid_360_intrinsic_calibration PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( livox_mid_360_intrinsic_calibration - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include diff --git a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt index 360fd81b..5e757086 100644 --- a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt +++ b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt @@ -36,13 +36,13 @@ 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_compile_definitions(mandeye_mission_recorder_calibration PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( mandeye_mission_recorder_calibration - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index 971360d3..e41f2072 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -39,13 +39,13 @@ 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_compile_definitions(mandeye_raw_data_viewer PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( mandeye_raw_data_viewer - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include 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 66be38b4..99070f49 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -97,7 +97,7 @@ int index_rendered_points_local = -1; // std::vector> all_points_local; // std::vector> all_lidar_ids; std::vector indexes_to_filename; -double ahrs_gain = 0.5; +double vqf_tauAcc = 0.5; double wx = 1000000.0; double wy = 1000000.0; double wz = 1000000.0; @@ -887,7 +887,7 @@ void loadFiles(std::vector input_file_names) params.filter_threshold_xy_outer = filter_threshold_xy_outer; std::vector> pointsPerFile; - std::vector, FusionVector, FusionVector>> imu_data; + Imu imu_data; // no files selected, quit loading if (input_file_names.empty()) @@ -964,104 +964,70 @@ void loadFiles(std::vector input_file_names) // rest of RAW data viewer processing - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - ahrs.settings.convention = FusionConventionNwu; - - if (fusionConventionEnu) - ahrs.settings.convention = FusionConventionEnu; - - if (fusionConventionNed) - ahrs.settings.convention = FusionConventionNed; + // VQF initialization + double avg_dt = SAMPLE_PERIOD; + if (imu_data.size() >= 2) + { + double t0 = std::get<0>(imu_data.front()).first; + double t1 = std::get<0>(imu_data.back()).first; + if (t1 > t0) + avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); + } - ahrs.settings.gain = ahrs_gain; + VQFParams vqf_params; + vqf_params.tauAcc = vqf_tauAcc > 0.0 ? vqf_tauAcc : 3.0; + VQF vqf(vqf_params, avg_dt); std::map> trajectory; int counter = 1; - static bool first = true; - - static double last_ts; for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * 180.0 / M_PI), - static_cast(gyr.axis.y * 180.0 / M_PI), - static_cast(gyr.axis.z * 180.0 / M_PI) }; - // const FusionVector gyroscope = {static_cast(gyr.axis.x), static_cast(gyr.axis.y), - // static_cast(gyr.axis.z)}; - const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - first = false; - // last_ts = timestamp_pair.first; - } - else - { - double curr_ts = timestamp_pair.first; - - double ts_diff = curr_ts - last_ts; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - - /*if (ts_diff < 0) - { - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - } - - if (ts_diff < 0.01) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - else - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - }*/ - } - - last_ts = timestamp_pair.first; - // - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + const double g = 9.81; + vqf_real_t gyr_vqf[3] = { + static_cast(gyr.x()), + static_cast(gyr.y()), + static_cast(gyr.z()) }; + vqf_real_t acc_vqf[3] = { + static_cast(acc.x()) * g, + static_cast(acc.y()) * g, + static_cast(acc.z()) * g }; + + vqf.update(gyr_vqf, acc_vqf); + + vqf_real_t quat[4]; + vqf.getQuat6D(quat); + Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]); Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; t.rotate(d); trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); + + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); counter++; if (counter % 100 == 0) { spdlog::info( "[{} of {}]: Roll {}, Pitch {}, Yaw {}", - counter++, + counter, imu_data.size(), - euler.angle.roll, - euler.angle.pitch, - euler.angle.yaw); + euler.x(), + euler.y(), + euler.z()); } // log it for implot imu_data_plot.timestampLidar.push_back(timestamp_pair.first); - imu_data_plot.angX.push_back(gyr.axis.x); - imu_data_plot.angY.push_back(gyr.axis.y); - imu_data_plot.angZ.push_back(gyr.axis.z); - imu_data_plot.accX.push_back(acc.axis.x); - imu_data_plot.accY.push_back(acc.axis.y); - imu_data_plot.accZ.push_back(acc.axis.z); - imu_data_plot.yaw.push_back(euler.angle.yaw); - imu_data_plot.pitch.push_back(euler.angle.pitch); - imu_data_plot.roll.push_back(euler.angle.roll); + imu_data_plot.angX.push_back(gyr.x()); + imu_data_plot.angY.push_back(gyr.y()); + imu_data_plot.angZ.push_back(gyr.z()); + imu_data_plot.accX.push_back(acc.x()); + imu_data_plot.accY.push_back(acc.y()); + imu_data_plot.accZ.push_back(acc.z()); + imu_data_plot.yaw.push_back(euler.z()); + imu_data_plot.pitch.push_back(euler.y()); + imu_data_plot.roll.push_back(euler.x()); } std::vector> timestamps; @@ -1339,13 +1305,13 @@ void settings_gui() number_of_points_threshold = 0; } - ImGui::InputDouble("AHRS gain", &ahrs_gain); + ImGui::InputDouble("VQF tauAcc [s]", &vqf_tauAcc); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Parameter for AHRS (Attitude and Heading Reference System) filter"); - ImGui::Text("Controls how strongly the filter corrects its orientation estimate"); - ImGui::Text("using accelerometer and magnetometer data, relative to the gyroscope integration"); + 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(); } diff --git a/apps/mandeye_single_session_viewer/CMakeLists.txt b/apps/mandeye_single_session_viewer/CMakeLists.txt index 57396796..0313b5fa 100644 --- a/apps/mandeye_single_session_viewer/CMakeLists.txt +++ b/apps/mandeye_single_session_viewer/CMakeLists.txt @@ -35,11 +35,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_single_session_viewer - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog ${FREEGLUT_LIBRARY} diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 476b40b7..b1386a2a 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -37,7 +37,7 @@ 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 ${EXTERNAL_LIBRARIES_DIRECTORY}/include) target_link_libraries( diff --git a/apps/quick_start_demo/CMakeLists.txt b/apps/quick_start_demo/CMakeLists.txt index 429563f8..b14c9576 100644 --- a/apps/quick_start_demo/CMakeLists.txt +++ b/apps/quick_start_demo/CMakeLists.txt @@ -26,13 +26,13 @@ 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_compile_definitions(quick_start_demo PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( quick_start_demo - PRIVATE Fusion + PRIVATE vqf spdlog::spdlog UTL::include ${FREEGLUT_LIBRARY} diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index 0caa445d..1f9779e9 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/apps/quick_start_demo/quick_start_demo.cpp @@ -832,7 +832,7 @@ int main(int argc, char *argv[]) std::cout << input_file_names[i] << std::endl; } std::cout << "loading imu" << std::endl; - std::vector, FusionVector, FusionVector>> imu_data; + std::vector, Eigen::Vector3f, Eigen::Vector3f>> imu_data; for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) { @@ -896,44 +896,40 @@ int main(int argc, char *argv[]) }); std::cout << "std::transform finished" << std::endl; - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - { - ahrs.settings.convention = FusionConventionNwu; - } - if (fusionConventionEnu) - { - ahrs.settings.convention = FusionConventionEnu; - } - if (fusionConventionNed) - { - ahrs.settings.convention = FusionConventionNed; - } + // VQF initialization + VQFParams vqf_params; + vqf_params.tauAcc = 3.0; + VQF vqf(vqf_params, SAMPLE_PERIOD); std::map> trajectory; int counter = 1; for (const auto &[timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = {static_cast(gyr.axis.x * 180.0 / M_PI), static_cast(gyr.axis.y * 180.0 / M_PI), static_cast(gyr.axis.z * 180.0 / M_PI)}; - const FusionVector accelerometer = {acc.axis.x, acc.axis.y, acc.axis.z}; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{quat.element.w, quat.element.x, quat.element.y, quat.element.z}; + const double g = 9.81; + vqf_real_t gyr_vqf[3] = { + static_cast(gyr.x()), + static_cast(gyr.y()), + static_cast(gyr.z()) }; + vqf_real_t acc_vqf[3] = { + static_cast(acc.x()) * g, + static_cast(acc.y()) * g, + static_cast(acc.z()) * g }; + + vqf.update(gyr_vqf, acc_vqf); + + vqf_real_t quat[4]; + vqf.getQuat6D(quat); + Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]); Eigen::Affine3d t{Eigen::Matrix4d::Identity()}; t.rotate(d); trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); counter++; if (counter % 100 == 0) { - std::cout << "Roll " << euler.angle.roll<< ", Pitch " << euler.angle.pitch<< ", Yaw " << euler.angle.yaw<< " [" << counter++ << " of " << imu_data.size() << "]"<< std::endl; + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); + std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl; } } diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index d6109a6e..eb54f12b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -9,6 +9,7 @@ set(CORE_BASE_SOURCES src/ground_control_points.cpp src/hash_utils.cpp src/icp.cpp + src/imu_preintegration.cpp src/ndt.cpp src/nmea.cpp src/optimization_point_to_point_source_to_target.cpp @@ -45,7 +46,7 @@ function(add_core_target target_name with_gui) add_library(${target_name} STATIC ${SOURCES}) target_compile_definitions(${target_name} PRIVATE ${DEFINES}) - target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog) + target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog vqf) target_include_directories(${target_name} PRIVATE include ${EIGEN3_INCLUDE_DIR} @@ -53,8 +54,9 @@ function(add_core_target target_name with_gui) ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${EXTERNAL_LIBRARIES_DIRECTORY}/include - ) - + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ) + target_link_libraries(${target_name} PRIVATE PROJ::proj) if(${with_gui}) diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h new file mode 100644 index 00000000..7fa27f02 --- /dev/null +++ b/core/include/imu_preintegration.h @@ -0,0 +1,154 @@ +#pragma once + +#include +#include +#include +#include + +struct IntegrationParams +{ + bool accel_units_in_g = true; + bool gyro_units_in_deg_per_sec = true; + double max_acceleration_threshold = 50.0; // m/s^2 + double max_dt_threshold = 0.1; // seconds + Eigen::Vector3d initial_velocity = Eigen::Vector3d::Zero(); + double vqf_tauAcc = 0.5; // VQF accelerometer time constant [s] +}; + +namespace imu_utils +{ +Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g = 9.81); +Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg); +double safe_dt(double t_prev, double t_curr, double max_dt); +bool has_nan(const Eigen::Vector3d& v); +bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold); +std::vector estimate_orientations( + const std::vector& raw_imu_data, + const Eigen::Matrix3d& initial_orientation, + const IntegrationParams& params); +} // namespace imu_utils + +class AccelerationModel +{ +public: + virtual ~AccelerationModel() = default; + + virtual Eigen::Vector3d compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const = 0; +}; + +class BodyFrameAcceleration : public AccelerationModel +{ +public: + Eigen::Vector3d compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const override; +}; + +class GravityCompensatedAcceleration : public AccelerationModel +{ +public: + double gravity_magnitude = 9.81; + Eigen::Vector3d gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81); + + Eigen::Vector3d compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const override; +}; + +class IntegrationMethod +{ +public: + virtual ~IntegrationMethod() = default; + + virtual Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const = 0; +}; + +class EulerIntegration : public IntegrationMethod +{ +public: + Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const override; +}; + +class TrapezoidalIntegration : public IntegrationMethod +{ +public: + Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const override; +}; + +class KalmanFilterIntegration : public IntegrationMethod +{ +public: + double process_noise_accel = 0.5; + double process_noise_bias = 0.01; + double measurement_noise_velocity = 1.0; + Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero(); + + Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const override; +}; + +enum class PreintegrationMethod +{ + euler_body_frame = 0, + trapezoidal_body_frame = 1, + euler_gravity_compensated = 2, + trapezoidal_gravity_compensated = 3, + kalman_filter = 4, + euler_gyro_gravity_compensated = 5, + trapezoidal_gyro_gravity_compensated = 6, + kalman_gyro_gravity_compensated = 7, +}; + +inline const char* to_string(PreintegrationMethod method) +{ + switch (method) + { + case PreintegrationMethod::euler_body_frame: return "Euler, no gravity compensation"; + case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal, no gravity compensation"; + case PreintegrationMethod::euler_gravity_compensated: return "Euler, gravity comp. (initial trajectory orientations)"; + case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal, gravity comp. (initial trajectory orientations)"; + case PreintegrationMethod::kalman_filter: return "Kalman, gravity comp. (initial trajectory orientations)"; + case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler, gravity comp. (per-worker VQF orientations)"; + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal, gravity comp. (per-worker VQF orientations)"; + case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman, gravity comp. (per-worker VQF orientations)"; + default: return "unknown"; + } +} + +class ImuPreintegration +{ +public: + IntegrationParams params; + + Eigen::Vector3d preintegrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationMethod& integration_method); + + static Eigen::Vector3d create_and_preintegrate( + PreintegrationMethod method, + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const IntegrationParams& params = IntegrationParams()); +}; diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp new file mode 100644 index 00000000..e800efbd --- /dev/null +++ b/core/src/imu_preintegration.cpp @@ -0,0 +1,336 @@ +#include + +#include +#include +#include + +#include + +namespace imu_utils +{ + +Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g) +{ + if (units_in_g) + return raw * g; + return raw; +} + +Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg) +{ + if (units_in_deg) + return raw * (M_PI / 180.0); + return raw; +} + +double safe_dt(double t_prev, double t_curr, double max_dt) +{ + double dt = t_curr - t_prev; + if (dt <= 0.0 || std::isnan(dt)) + return 0.0; + return std::min(dt, max_dt); +} + +bool has_nan(const Eigen::Vector3d& v) +{ + return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z()); +} + +bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold) +{ + return accel_ms2.norm() < threshold && !has_nan(accel_ms2); +} + +std::vector estimate_orientations( + const std::vector& raw_imu_data, + const Eigen::Matrix3d& initial_orientation, + const IntegrationParams& params) +{ + std::vector orientations; + orientations.reserve(raw_imu_data.size()); + + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; // default 200 Hz + if (raw_imu_data.size() >= 2) + { + double total_time = raw_imu_data.back().timestamp - raw_imu_data.front().timestamp; + if (total_time > 0.0) + avg_dt = total_time / static_cast(raw_imu_data.size() - 1); + } + + VQFParams vqf_params; + vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; + VQF vqf(vqf_params, avg_dt); + + orientations.push_back(initial_orientation); + + for (size_t k = 1; k < raw_imu_data.size(); k++) + { + double dt = safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + { + orientations.push_back(orientations.back()); + continue; + } + + // VQF expects: gyro in rad/s, acc in m/s² + // RawIMUData: gyro in deg/s, accel in g + const double deg2rad = M_PI / 180.0; + const double g = 9.81; + vqf_real_t gyr[3] = { + raw_imu_data[k].guroscopes.x() * deg2rad, + raw_imu_data[k].guroscopes.y() * deg2rad, + raw_imu_data[k].guroscopes.z() * deg2rad }; + vqf_real_t acc[3] = { + raw_imu_data[k].accelerometers.x() * g, + raw_imu_data[k].accelerometers.y() * g, + raw_imu_data[k].accelerometers.z() * g }; + + vqf.update(gyr, acc); + + vqf_real_t quat[4]; // [w, x, y, z] + vqf.getQuat6D(quat); + Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]); + orientations.push_back(q.toRotationMatrix()); + } + return orientations; +} + +} // namespace imu_utils + +Eigen::Vector3d BodyFrameAcceleration::compute( + const RawIMUData& imu, + const Eigen::Affine3d& /*pose*/, + const IntegrationParams& params) const +{ + return imu_utils::convert_accel_to_ms2(imu.accelerometers, params.accel_units_in_g); +} + +Eigen::Vector3d GravityCompensatedAcceleration::compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const +{ + Eigen::Vector3d a_body = imu_utils::convert_accel_to_ms2(imu.accelerometers, params.accel_units_in_g, gravity_magnitude); + Eigen::Matrix3d R = pose.rotation(); + return R * a_body + gravity_vector; +} + +// v_{k} = v_{k-1} + a_k * dt +// p_{k} = p_{k-1} + v_{k} * dt +Eigen::Vector3d EulerIntegration::integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const +{ + const size_t n = raw_imu_data.size(); + Eigen::Vector3d velocity = params.initial_velocity; + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + + for (size_t k = 1; k < n; k++) + { + double dt = imu_utils::safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + continue; + + Eigen::Vector3d accel = accel_model.compute(raw_imu_data[k], new_trajectory[k], params); + if (!imu_utils::is_accel_valid(accel, params.max_acceleration_threshold)) + accel = Eigen::Vector3d::Zero(); + + velocity += accel * dt; + position += velocity * dt; + } + + return position; +} + +// v_{k} = v_{k-1} + 0.5 * (a_{k-1} + a_k) * dt +// p_{k} = p_{k-1} + 0.5 * (v_{k-1} + v_k) * dt +Eigen::Vector3d TrapezoidalIntegration::integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const +{ + const size_t n = raw_imu_data.size(); + Eigen::Vector3d velocity = params.initial_velocity; + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + + Eigen::Vector3d prev_accel = accel_model.compute(raw_imu_data[0], new_trajectory[0], params); + if (!imu_utils::is_accel_valid(prev_accel, params.max_acceleration_threshold)) + prev_accel = Eigen::Vector3d::Zero(); + + for (size_t k = 1; k < n; k++) + { + double dt = imu_utils::safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + continue; + + Eigen::Vector3d curr_accel = accel_model.compute(raw_imu_data[k], new_trajectory[k], params); + if (!imu_utils::is_accel_valid(curr_accel, params.max_acceleration_threshold)) + curr_accel = prev_accel; + + Eigen::Vector3d prev_velocity = velocity; + velocity += 0.5 * (prev_accel + curr_accel) * dt; + position += 0.5 * (prev_velocity + velocity) * dt; + + prev_accel = curr_accel; + } + + return position; +} + +// EKF state: x = [position(3), velocity(3), accel_bias(3)]^T (9-dim) +// F = [I, I*dt, 0; 0, I, -I*dt; 0, 0, I] +// Measurement: velocity constraint H = [0, I, 0], z = initial_velocity +Eigen::Vector3d KalmanFilterIntegration::integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const +{ + const size_t n = raw_imu_data.size(); + + Eigen::Matrix state = Eigen::Matrix::Zero(); + state.segment<3>(0) = Eigen::Vector3d::Zero(); + state.segment<3>(3) = params.initial_velocity; + state.segment<3>(6) = initial_accel_bias; + + Eigen::Matrix P = Eigen::Matrix::Identity(); + P.block<3, 3>(0, 0) *= 0.01; + P.block<3, 3>(3, 3) *= 1.0; + P.block<3, 3>(6, 6) *= 0.1; + + double sigma_a = process_noise_accel; + double sigma_b = process_noise_bias; + + for (size_t k = 1; k < n; k++) + { + double dt = imu_utils::safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + continue; + + Eigen::Vector3d accel = accel_model.compute(raw_imu_data[k], new_trajectory[k], params); + if (!imu_utils::is_accel_valid(accel, params.max_acceleration_threshold)) + accel = Eigen::Vector3d::Zero(); + + Eigen::Vector3d bias = state.segment<3>(6); + Eigen::Vector3d a_corrected = accel - bias; + + Eigen::Matrix F = Eigen::Matrix::Identity(); + F.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity() * dt; + F.block<3, 3>(3, 6) = -Eigen::Matrix3d::Identity() * dt; + + Eigen::Matrix state_pred; + state_pred.segment<3>(0) = state.segment<3>(0) + state.segment<3>(3) * dt + 0.5 * a_corrected * dt * dt; + state_pred.segment<3>(3) = state.segment<3>(3) + a_corrected * dt; + state_pred.segment<3>(6) = state.segment<3>(6); + + Eigen::Matrix Q = Eigen::Matrix::Zero(); + Q.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * (sigma_a * sigma_a * dt * dt * dt * dt / 4.0); + Q.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * (sigma_a * sigma_a * dt * dt); + Q.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * (sigma_b * sigma_b * dt); + + Eigen::Matrix P_pred = F * P * F.transpose() + Q; + + Eigen::Matrix H = Eigen::Matrix::Zero(); + H.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix3d R_meas = Eigen::Matrix3d::Identity() * (measurement_noise_velocity * measurement_noise_velocity); + Eigen::Matrix3d S = H * P_pred * H.transpose() + R_meas; + Eigen::Matrix K = P_pred * H.transpose() * S.inverse(); + + Eigen::Vector3d innovation = params.initial_velocity - state_pred.segment<3>(3); + + state = state_pred + K * innovation; + P = (Eigen::Matrix::Identity() - K * H) * P_pred; + } + + return state.segment<3>(0); +} + +Eigen::Vector3d ImuPreintegration::preintegrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationMethod& integration_method) +{ + if (raw_imu_data.size() < 2 || new_trajectory.size() < 2) + return Eigen::Vector3d::Zero(); + + if (raw_imu_data.size() != new_trajectory.size()) + return Eigen::Vector3d::Zero(); + + Eigen::Vector3d total_displacement = integration_method.integrate( + raw_imu_data, new_trajectory, accel_model, params); + + if (imu_utils::has_nan(total_displacement)) + return Eigen::Vector3d::Zero(); + + Eigen::Vector3d mean_shift = total_displacement / static_cast(raw_imu_data.size() - 1); + return mean_shift; +} + +Eigen::Vector3d ImuPreintegration::create_and_preintegrate( + PreintegrationMethod method, + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const IntegrationParams& params) +{ + ImuPreintegration preint; + preint.params = params; + + std::unique_ptr accel_model; + std::unique_ptr integration_method; + + switch (method) + { + case PreintegrationMethod::euler_body_frame: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::trapezoidal_body_frame: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::euler_gravity_compensated: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::trapezoidal_gravity_compensated: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::kalman_filter: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::euler_gyro_gravity_compensated: + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: + case PreintegrationMethod::kalman_gyro_gravity_compensated: + { + auto orientations = imu_utils::estimate_orientations( + raw_imu_data, new_trajectory[0].rotation(), params); + + std::vector imu_trajectory = new_trajectory; + for (size_t k = 0; k < imu_trajectory.size() && k < orientations.size(); k++) + imu_trajectory[k].linear() = orientations[k]; + + accel_model = std::make_unique(); + if (method == PreintegrationMethod::euler_gyro_gravity_compensated) + integration_method = std::make_unique(); + else if (method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated) + integration_method = std::make_unique(); + else + integration_method = std::make_unique(); + + return preint.preintegrate(raw_imu_data, imu_trajectory, *accel_model, *integration_method); + } + default: + std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl; + return Eigen::Vector3d::Zero(); + } + + return preint.preintegrate(raw_imu_data, new_trajectory, *accel_model, *integration_method); +} diff --git a/pybind/CMakeLists.txt b/pybind/CMakeLists.txt index 4f93b3e2..a08647a7 100644 --- a/pybind/CMakeLists.txt +++ b/pybind/CMakeLists.txt @@ -76,7 +76,7 @@ target_include_directories(core_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY} ${THIRDPARTY_DIRECTORY}/glm @@ -90,7 +90,7 @@ target_include_directories(lidar_odometry_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/tomlplusplus/include ${THIRDPARTY_DIRECTORY} @@ -105,7 +105,7 @@ target_include_directories(multi_view_tls_registration_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY} ${THIRDPARTY_DIRECTORY}/glm @@ -116,7 +116,7 @@ target_link_libraries(core_py PRIVATE pybind11::module core_no_gui ${PLATFORM_LASZIP_LIB} - Fusion + vqf ${PLATFORM_MISCELLANEOUS_LIBS} ) @@ -128,7 +128,7 @@ target_link_libraries(lidar_odometry_py spdlog::spdlog UTL::include ${PLATFORM_LASZIP_LIB} - Fusion + vqf ${PLATFORM_MISCELLANEOUS_LIBS} ) @@ -136,7 +136,7 @@ target_link_libraries(multi_view_tls_registration_py PRIVATE pybind11::module core_no_gui ${PLATFORM_LASZIP_LIB} - Fusion + vqf ${PLATFORM_MISCELLANEOUS_LIBS} ) diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp index fc1f581c..87c4aa02 100644 --- a/pybind/lidar_odometry_binding.cpp +++ b/pybind/lidar_odometry_binding.cpp @@ -100,7 +100,7 @@ PYBIND11_MODULE(lidar_odometry_py, m) .def_readwrite("use_mutliple_gaussian", &LidarOdometryParams::use_mutliple_gaussian) .def_readwrite("num_constistency_iter", &LidarOdometryParams::num_constistency_iter) .def_readwrite("threshould_output_filter", &LidarOdometryParams::threshould_output_filter) - .def_readwrite("ahrs_gain", &LidarOdometryParams::ahrs_gain) + .def_readwrite("vqf_tauAcc", &LidarOdometryParams::vqf_tauAcc) .def_readwrite("threshold_nr_poses", &LidarOdometryParams::threshold_nr_poses) .def_readwrite("current_output_dir", &LidarOdometryParams::current_output_dir) //.def_readwrite("min_counter", &LidarOdometryParams::min_counter)