diff --git a/3rdparty/Fusion b/3rdparty/Fusion deleted file mode 160000 index 53c67a79..00000000 --- a/3rdparty/Fusion +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 53c67a79e0d08b82ffa975af28631dbb2a68529c diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index c78554b9..e5d3c35b 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -145,6 +145,7 @@ int dec_reference_points = 100; bool show_initial_points = true; bool show_trajectory = true; bool show_trajectory_as_axes = false; +bool show_prediction_vectors = false; bool show_covs_indoor = false; bool show_covs_outdoor = false; int dec_covs = 10; @@ -916,14 +917,15 @@ void settings_gui() 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)" }; + const char* methods[] = { + "Euler, no gravity comp., SM velocity", + "Trapezoidal, no gravity comp., SM velocity", + "Euler, gravity comp., SM velocity", + "Trapezoidal, gravity comp., SM velocity", + "Kalman, gravity comp., SM velocity", + "Euler, gravity comp., VQF velocity", + "Trapezoidal, gravity comp., VQF velocity", + "Kalman, gravity comp., VQF velocity" }; 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"); @@ -959,21 +961,6 @@ void settings_gui() "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"); @@ -988,31 +975,29 @@ void settings_gui() "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"); + ImGui::Checkbox("Rest bias estimation", ¶ms.vqf_restBiasEstEnabled); if (ImGui::IsItemHovered()) - ImGui::SetTooltip( - "Acceleration threshold for rest detection.\nDeviation norm between measurement and reference must be below " - "threshold.\nDefault: 0.5"); + 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("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(); } @@ -1919,6 +1904,28 @@ void display() glEnd(); } + if (show_prediction_vectors) + { + glDisable(GL_DEPTH_TEST); + glLineWidth(3.0f); + glBegin(GL_LINES); + for (const auto& wd : worker_data) + { + if (wd.intermediate_trajectory.empty() || wd.imu_prediction_vector.norm() < 1e-6) + continue; + + Eigen::Vector3d start = wd.intermediate_trajectory.front().translation(); + Eigen::Vector3d end = start + wd.imu_prediction_vector; + + glColor3f(1.0f, 0.0f, 1.0f); // magenta + glVertex3d(start.x(), start.y(), start.z()); + glVertex3d(end.x(), end.y(), end.z()); + } + glEnd(); + glLineWidth(1.0f); + glEnable(GL_DEPTH_TEST); + } + if (show_trajectory) { glPointSize(3); @@ -2262,6 +2269,7 @@ void display() ImGui::MenuItem("Show initial points", nullptr, &show_initial_points); ImGui::MenuItem("Show trajectory", nullptr, &show_trajectory); ImGui::MenuItem("Show trajectory as axes", nullptr, &show_trajectory_as_axes); + ImGui::MenuItem("Show prediction vectors", nullptr, &show_prediction_vectors); ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index df7bd185..e4985b51 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -188,9 +188,8 @@ 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 + bool use_imu_preintegration = false; + 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; 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 2ea42550..86813c99 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2416,41 +2416,54 @@ bool process_worker_step_1( if (params.use_imu_preintegration) { IntegrationParams imu_params; + auto method = static_cast(params.imu_preintegration_method); - // 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(); - + // Compute IMU time span for velocity estimation + double total_imu_time = 0.0; if (worker_data.raw_imu_data.size() >= 2) + total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; + + if (total_imu_time > 0.0) { - double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; - if (total_imu_time > 0.0) + bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gravity_vqf_vel || + method == PreintegrationMethod::trapezoidal_gravity_vqf_vel || + method == PreintegrationMethod::kalman_gravity_vqf_vel); + + if (uses_vqf_velocity) { + // Methods 5-7: SM-independent velocity + // Direction from VQF AHRS, speed from motion model displacement + Eigen::Vector3d prev_mm_displacement = + prev_worker_data.intermediate_trajectory_motion_model.back().translation() - + prev_worker_data.intermediate_trajectory_motion_model.front().translation(); 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 + Eigen::Vector3d forward_global = worker_data.intermediate_trajectory[0].linear() * Eigen::Vector3d(1, 0, 0); + forward_global.z() = 0; if (forward_global.norm() > 1e-6) imu_params.initial_velocity = forward_global.normalized() * speed; else imu_params.initial_velocity = Eigen::Vector3d::Zero(); } + else + { + // Methods 0-4: velocity from previous SM trajectory + Eigen::Vector3d prev_displacement = + prev_worker_data.intermediate_trajectory.back().translation() - + prev_prev_worker_data.intermediate_trajectory.back().translation(); + imu_params.initial_velocity = prev_displacement / total_imu_time; + } } mean_shift = ImuPreintegration::create_and_preintegrate( - static_cast(params.imu_preintegration_method), worker_data.raw_imu_data, new_trajectory, imu_params); + 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 /= (prev_worker_data.intermediate_trajectory.size()); + // No preintegration: simple velocity from previous two workers + mean_shift = (prev_worker_data.intermediate_trajectory.back().translation() - + prev_prev_worker_data.intermediate_trajectory.back().translation()) / + static_cast(prev_worker_data.intermediate_trajectory.size()); } if (mean_shift.norm() > 1.0) @@ -2459,6 +2472,9 @@ bool process_worker_step_1( mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); } + // Store prediction vector for visualization (total displacement over worker) + worker_data.imu_prediction_vector = mean_shift * static_cast(new_trajectory.size()); + for (int tr = 0; tr < new_trajectory.size(); tr++) { new_trajectory[tr].translation() += mean_shift * tr; diff --git a/core/include/Core/structures.h b/core/include/Core/structures.h index d2d56be5..8bdfb0a6 100644 --- a/core/include/Core/structures.h +++ b/core/include/Core/structures.h @@ -426,5 +426,6 @@ struct WorkerData std::vector> intermediate_trajectory_timestamps; std::vector imu_om_fi_ka; std::vector raw_imu_data; + Eigen::Vector3d imu_prediction_vector = Eigen::Vector3d::Zero(); bool show = false; }; diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 4d324443..554f3994 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -17,13 +17,11 @@ struct IntegrationParams 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); +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); } // namespace imu_utils class AccelerationModel @@ -98,38 +96,29 @@ class KalmanFilterIntegration : public IntegrationMethod 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, + euler_no_gravity_sm_vel = 0, + trapezoidal_no_gravity_sm_vel = 1, + euler_gravity_sm_vel = 2, + trapezoidal_gravity_sm_vel = 3, + kalman_gravity_sm_vel = 4, + euler_gravity_vqf_vel = 5, + trapezoidal_gravity_vqf_vel = 6, + kalman_gravity_vqf_vel = 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"; + case PreintegrationMethod::euler_no_gravity_sm_vel: return "Euler, no gravity comp., SM velocity"; + case PreintegrationMethod::trapezoidal_no_gravity_sm_vel: return "Trapezoidal, no gravity comp., SM velocity"; + case PreintegrationMethod::euler_gravity_sm_vel: return "Euler, gravity comp., SM velocity"; + case PreintegrationMethod::trapezoidal_gravity_sm_vel: return "Trapezoidal, gravity comp., SM velocity"; + case PreintegrationMethod::kalman_gravity_sm_vel: return "Kalman, gravity comp., SM velocity"; + case PreintegrationMethod::euler_gravity_vqf_vel: return "Euler, gravity comp., VQF velocity"; + case PreintegrationMethod::trapezoidal_gravity_vqf_vel: return "Trapezoidal, gravity comp., VQF velocity"; + case PreintegrationMethod::kalman_gravity_vqf_vel: return "Kalman, gravity comp., VQF velocity"; + default: return "unknown"; } } diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index 050b9ed6..fecb7efe 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -4,8 +4,6 @@ #include #include -#include - namespace imu_utils { @@ -16,82 +14,6 @@ namespace imu_utils 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( @@ -277,46 +199,41 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( switch (method) { - case PreintegrationMethod::euler_body_frame: + case PreintegrationMethod::euler_no_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::trapezoidal_body_frame: + case PreintegrationMethod::trapezoidal_no_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::euler_gravity_compensated: + case PreintegrationMethod::euler_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::trapezoidal_gravity_compensated: + case PreintegrationMethod::trapezoidal_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::kalman_filter: + case PreintegrationMethod::kalman_gravity_sm_vel: 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); - } + case PreintegrationMethod::euler_gravity_vqf_vel: + case PreintegrationMethod::trapezoidal_gravity_vqf_vel: + case PreintegrationMethod::kalman_gravity_vqf_vel: + { + // Use orientations from new_trajectory directly (global VQF from calculate_trajectory, + // anchored to previous worker's SM pose). No per-worker VQF needed. + accel_model = std::make_unique(); + if (method == PreintegrationMethod::euler_gravity_vqf_vel) + integration_method = std::make_unique(); + else if (method == PreintegrationMethod::trapezoidal_gravity_vqf_vel) + integration_method = std::make_unique(); + else + integration_method = std::make_unique(); + break; + } default: std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl; return Eigen::Vector3d::Zero();