From 657aa600b794e260e750a6240f36fa4e76ca2b3a Mon Sep 17 00:00:00 2001 From: Mitchell Date: Tue, 25 Nov 2025 15:40:00 -0500 Subject: [PATCH] Simplify preintegration code --- CMakeLists.txt | 1 - examples/gps_imu_example.cpp | 54 ++++--- examples/include/FactorGraphUtils.h | 4 +- examples/include/GPSIMUExampleUtils.h | 27 ++-- examples/python/run_gps_imu_fusion.py | 1 + include/imu/IMUHelper.h | 40 ----- include/imu/IMUIncrement.h | 183 ++++++++++++++++------ include/imu/IMUPreintegrationHelper.h | 13 +- src/imu/IMUHelper.cpp | 151 ------------------- src/imu/IMUIncrement.cpp | 159 +++++++++++--------- src/imu/IMUPreintegrationHelper.cpp | 208 +++++++++++++------------- src/lie/SE3.cpp | 6 +- tests/test_factor_graph.cpp | 16 +- tests/test_jacobians.cpp | 45 ++++-- 14 files changed, 444 insertions(+), 464 deletions(-) delete mode 100644 include/imu/IMUHelper.h delete mode 100644 src/imu/IMUHelper.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a1fd744..918ffd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,6 @@ list( src/lie/SO3.cpp src/imu/IMUIncrement.cpp - src/imu/IMUHelper.cpp src/imu/IMUPreintegrationHelper.cpp src/utils/Utils.cpp diff --git a/examples/gps_imu_example.cpp b/examples/gps_imu_example.cpp index 791833e..e807b53 100644 --- a/examples/gps_imu_example.cpp +++ b/examples/gps_imu_example.cpp @@ -70,8 +70,8 @@ void runSlidingWindowEstimator( const std::vector &gps_data, const IMUState &init_imu_state, const Eigen::Matrix &init_cov, LieDirection lie_direction, ExtendedPoseRepresentation state_rep, - const Eigen::Matrix &Q_ct, const Eigen::Matrix3d &R_gps, - const Eigen::Vector3d &gravity, const std::string &est_imu_file, + std::shared_ptr preint_options, + const Eigen::Matrix3d &R_gps, const std::string &est_imu_file, const std::string &cov_file, int window_size) { ceres::Solver::Options options; options.max_num_iterations = 10; @@ -85,14 +85,14 @@ void runSlidingWindowEstimator( factor_graph_utils::ProblemKeys keys; // Add the first IMU state to the graph and add a prior factor - factor_graph_utils::addIMUState(graph, init_imu_state, lie_direction, state_rep); + factor_graph_utils::addIMUState(graph, init_imu_state, lie_direction, + state_rep); factor_graph_utils::addPriorFactor(graph, init_imu_state, init_cov, lie_direction, state_rep, keys); // Create the initial IMU increment - IMUIncrement imu_increment( - Q_ct, init_imu_state.gyroBias(), init_imu_state.accelBias(), - init_imu_state.timestamp(), gravity, lie_direction, state_rep); + IMUIncrement imu_increment(preint_options, init_imu_state.gyroBias(), + init_imu_state.accelBias()); IMUState cur_imu_state = init_imu_state; double prev_gps_timestamp = gps_data[0].timestamp; std::vector est_stamps; @@ -114,12 +114,13 @@ void runSlidingWindowEstimator( imu_increment.propagate(dt, imu_data[imu_idx].gyro, imu_data[imu_idx].accel); // Propagate IMU state and add to graph - propagateIMUState(cur_imu_state, imu_data[imu_idx], gravity, dt); + propagateIMUState(cur_imu_state, imu_data[imu_idx], preint_options->gravity, dt); imu_idx++; } // Add the new IMU state to the graph along with factors at the timestamp - factor_graph_utils::addIMUState(graph, cur_imu_state, lie_direction, state_rep, keys); + factor_graph_utils::addIMUState(graph, cur_imu_state, lie_direction, + state_rep, keys); factor_graph_utils::addPreintegrationFactor(graph, imu_increment, lie_direction, state_rep, keys); factor_graph_utils::addGPSFactor(graph, gps_data[gps_index], lie_direction, @@ -170,8 +171,8 @@ void runFullBatchEstimator( const std::vector &gps_data, const IMUState &init_imu_state, const Eigen::Matrix &init_cov, LieDirection lie_direction, ExtendedPoseRepresentation state_rep, - const Eigen::Matrix &Q_ct, const Eigen::Matrix3d &R_gps, - const Eigen::Vector3d &gravity, const std::string &est_imu_file, + std::shared_ptr preint_options, + const Eigen::Matrix3d &R_gps, const std::string &est_imu_file, const std::string &cov_file) { // Create the factor graph and problem keys @@ -185,9 +186,9 @@ void runFullBatchEstimator( // Create the initial IMU increment size_t imu_idx = 0; - IMUIncrement imu_increment( - Q_ct, init_imu_state.gyroBias(), init_imu_state.accelBias(), - init_imu_state.timestamp(), gravity, lie_direction, state_rep); + + IMUIncrement imu_increment(preint_options, init_imu_state.gyroBias(), + init_imu_state.accelBias()); IMUState cur_imu_state = init_imu_state; double prev_gps_timestamp = gps_data[0].timestamp; @@ -209,12 +210,14 @@ void runFullBatchEstimator( imu_increment.propagate(dt, imu_data[imu_idx].gyro, imu_data[imu_idx].accel); // Propagate IMU state and add to graph - propagateIMUState(cur_imu_state, imu_data[imu_idx], gravity, dt); + propagateIMUState(cur_imu_state, imu_data[imu_idx], + preint_options->gravity, dt); imu_idx++; } // Add the new IMU state to the graph along with factors at the timestamp - factor_graph_utils::addIMUState(graph, cur_imu_state, lie_direction, state_rep, keys); + factor_graph_utils::addIMUState(graph, cur_imu_state, lie_direction, + state_rep, keys); factor_graph_utils::addPreintegrationFactor(graph, imu_increment, lie_direction, state_rep, keys); factor_graph_utils::addGPSFactor(graph, gps_data[gps_index], lie_direction, @@ -306,12 +309,6 @@ int main(int argc, const char **argv) { double sigma_gyro_rw = args["sigma_gyro_random_walk_continuous"].as(); double sigma_accel_rw = args["sigma_accel_random_walk_continuous"].as(); - Eigen::Matrix Q_ct = - Eigen::Matrix::Identity(); - Q_ct.block<3, 3>(0, 0) *= sigma_gyro * sigma_gyro; - Q_ct.block<3, 3>(3, 3) *= sigma_accel * sigma_accel; - Q_ct.block<3, 3>(6, 6) *= sigma_gyro_rw * sigma_gyro_rw; - Q_ct.block<3, 3>(9, 9) *= sigma_accel_rw * sigma_accel_rw; double gravity_mag = args["gravity_mag"].as(); Eigen::Vector3d gravity = Eigen::Vector3d(0.0, 0.0, -gravity_mag); @@ -330,6 +327,17 @@ int main(int argc, const char **argv) { LOG(INFO) << "Running estimator type: " << estimator_type; + // Create our preintegration options + std::shared_ptr preint_options = + std::make_shared(); + preint_options->sigma_gyro_ct = sigma_gyro; + preint_options->sigma_accel_ct = sigma_accel; + preint_options->sigma_gyro_bias_ct = sigma_gyro_rw; + preint_options->sigma_accel_bias_ct = sigma_accel_rw; + preint_options->gravity = gravity; + preint_options->direction = lie_direction; + preint_options->pose_rep = state_rep; + // Create output files std::string output_dir = args["output_dir"].as(); std::string est_imu_states_file = output_dir + "/optimized_imu_states.txt"; @@ -342,13 +350,13 @@ int main(int argc, const char **argv) { Eigen::Matrix::Identity() * 1e-6; // Small covariance if (estimator_type == "full_batch") { runFullBatchEstimator(imu_data, gps_data, init_imu_state, init_cov, - lie_direction, state_rep, Q_ct, R_gps, gravity, + lie_direction, state_rep, preint_options, R_gps, est_imu_states_file, cov_file); } else { int sliding_window_size = args["sliding_window_size"].as(); LOG(INFO) << "Using sliding window size: " << sliding_window_size; runSlidingWindowEstimator(imu_data, gps_data, init_imu_state, init_cov, - lie_direction, state_rep, Q_ct, R_gps, gravity, + lie_direction, state_rep, preint_options, R_gps, est_imu_states_file, cov_file, sliding_window_size); } diff --git a/examples/include/FactorGraphUtils.h b/examples/include/FactorGraphUtils.h index b525fa9..d947e2e 100644 --- a/examples/include/FactorGraphUtils.h +++ b/examples/include/FactorGraphUtils.h @@ -64,8 +64,8 @@ void addPreintegrationFactor(ceres_nav::FactorGraph &graph, LieDirection &direction, ExtendedPoseRepresentation state_rep, ProblemKeys keys = ProblemKeys()) { - double start_stamp = imu_increment.start_stamp; - double end_stamp = imu_increment.end_stamp; + double start_stamp = imu_increment.startTime(); + double end_stamp = imu_increment.endTime(); std::vector state_ids = {StateID(keys.nav_state_key, start_stamp), StateID(keys.bias_state_key, start_stamp), StateID(keys.nav_state_key, end_stamp), diff --git a/examples/include/GPSIMUExampleUtils.h b/examples/include/GPSIMUExampleUtils.h index 281ce80..680118a 100644 --- a/examples/include/GPSIMUExampleUtils.h +++ b/examples/include/GPSIMUExampleUtils.h @@ -7,8 +7,8 @@ #include #include +#include "imu/IMUIncrement.h" #include "lie/SE23.h" -#include "imu/IMUHelper.h" #include @@ -70,9 +70,7 @@ class IMUState { nav_state_ = nav_state; } - void setStamp(double stamp) { - timestamp_ = stamp; - } + void setStamp(double stamp) { timestamp_ = stamp; } Eigen::Matrix toVector() const { Eigen::Matrix vec; @@ -228,17 +226,24 @@ std::vector loadIMUStates(const std::string &fname) { /** * @brief propagates and IMU state forward using the IMU measurements. -*/ -void propagateIMUState(IMUState &state, const IMUMessage &imu_msg, const Eigen::Vector3d &gravity, double dt) { + */ +void propagateIMUState(IMUState &state, const IMUMessage &imu_msg, + const Eigen::Vector3d &gravity, double dt) { Eigen::Vector3d unbiased_gyro = imu_msg.gyro - state.gyroBias(); Eigen::Vector3d unbiased_accel = imu_msg.accel - state.accelBias(); - Eigen::Matrix G = ceres_nav::createGMatrix(gravity, dt); - Eigen::Matrix U = - ceres_nav::createUMatrix(unbiased_gyro, unbiased_accel, dt); + Eigen::Matrix T_k = state.navState(); + + // Propagation written as the product of three SE_2(3) matrices: Gamma_k * Phi + // (T_k) * Upsilon_k + Eigen::Matrix Gamma_k = Eigen::Matrix::Identity(); + Gamma_k.block<3, 1>(0, 3) = dt * gravity; + Gamma_k.block<3, 1>(0, 4) = 0.5 * dt * dt * gravity; - Eigen::Matrix prev_extended_pose = state.navState(); - Eigen::Matrix next_extended_pose = G * prev_extended_pose * U; + Eigen::Matrix Phi_k = ceres_nav::phiMat(dt, T_k); + Eigen::Matrix Upsilon_k = ceres_nav::upsilonMat( + dt, unbiased_gyro, unbiased_accel, ceres_nav::IMUDiscretizationMethod::ConstantMeas); + Eigen::Matrix next_extended_pose = Gamma_k * Phi_k * Upsilon_k; double new_stamp = state.timestamp() + dt; state.setStamp(new_stamp); diff --git a/examples/python/run_gps_imu_fusion.py b/examples/python/run_gps_imu_fusion.py index 78d1c74..7ad8d1c 100644 --- a/examples/python/run_gps_imu_fusion.py +++ b/examples/python/run_gps_imu_fusion.py @@ -446,6 +446,7 @@ def evaluate_imu_states( config.lie_direction = "left" # left or right config.state_representation = "SE23" # SE23 or decoupled + config.estimator_type = "sliding_window" # sliding_window or full_batch # Generate data an run the example data_fpaths = generate_and_save_data(config, save_dir) diff --git a/include/imu/IMUHelper.h b/include/imu/IMUHelper.h deleted file mode 100644 index 8bda1f5..0000000 --- a/include/imu/IMUHelper.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -/* - * Some helper functions for IMU preintegration. - */ - -#include -#include - -namespace ceres_nav { -class IMUIncrement; -class IMU; - -Eigen::Matrix createGMatrix(const Eigen::Vector3d &gravity, - double dt); - -Eigen::Matrix3d createNMatrix(const Eigen::Vector3d &phi_vec); -Eigen::Matrix createUMatrix(const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel, - double dt); - -Eigen::Matrix inverseIE3(const Eigen::Matrix &X); -Eigen::Matrix createUMatrixInv(const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel, - double dt); - -Eigen::Matrix createLMatrix(const Eigen::Vector3d &unbiased_gyro, - const Eigen::Vector3d &unbiased_accel, - double dt); -Eigen::Matrix adjointIE3(const Eigen::Matrix &X); - -bool preintegrateIMUMeasurements(IMUIncrement &rmi, - const std::vector &imu_meas_vec); -std::vector getIMUBetweenTimes(const double &stamp_i, - const double &stamp_j, - const std::vector &imu_meas_vec); -bool preintegrateBetweenTimes(IMUIncrement &rmi, const double &stamp_i, - const double &stamp_j, - const std::vector &imu_meas_vec); -} // namespace ceres_name \ No newline at end of file diff --git a/include/imu/IMUIncrement.h b/include/imu/IMUIncrement.h index e0e7a12..41cca7c 100644 --- a/include/imu/IMUIncrement.h +++ b/include/imu/IMUIncrement.h @@ -7,69 +7,127 @@ namespace ceres_nav { -class IMUIncrement { +// Two options for mean discretization of the RMI are available - +// 1. ConstantAccel: here, we assume that the +// global acceleration is constant over the integration interval. +// 2. ConstantMeas: here, we assume that the IMU measurements themselves +// are constant over the integration interval. +enum class IMUDiscretizationMethod { ConstantAccel, ConstantMeas }; + +class IMUIncrementOptions { public: - // Initial gyro and accel bias - Eigen::Vector3d gyro_bias; - Eigen::Vector3d accel_bias; - - // Covariance and Jacobian that we'll propagate forward at each iteration - Eigen::Matrix covariance; - Eigen::Matrix jacobian; - - // Navigation state representation options + /** + * @brief Constructor for IMUIncrementOptions + */ + IMUIncrementOptions(double sigma_gyro_ct = 0.01, double sigma_accel_ct = 0.01, + double sigma_gyro_bias_ct = 0.001, + double sigma_accel_bias_ct = 0.001, + const Eigen::Vector3d &gravity = Eigen::Vector3d(0, 0, + -9.81), + const LieDirection &direction = LieDirection::left, + const ExtendedPoseRepresentation &pose_rep = + ExtendedPoseRepresentation::SE23, + const IMUDiscretizationMethod &discretization = + IMUDiscretizationMethod::ConstantMeas) + : direction(direction), pose_rep(pose_rep), sigma_gyro_ct(sigma_gyro_ct), + sigma_accel_ct(sigma_accel_ct), sigma_gyro_bias_ct(sigma_gyro_bias_ct), + sigma_accel_bias_ct(sigma_accel_bias_ct), gravity(gravity), + discretization(discretization){}; + + // State representation options LieDirection direction; - ExtendedPoseRepresentation pose_rep = ExtendedPoseRepresentation::SE23; + ExtendedPoseRepresentation pose_rep; + + // Noise covariances + double sigma_gyro_ct; + double sigma_accel_ct; + double sigma_gyro_bias_ct; + double sigma_accel_bias_ct; - // Gravity vector Eigen::Vector3d gravity; - // IMU noise parameters (continuous-time!) - Eigen::Matrix Q_ct; - // RMI - NOTE: not quite an element of SE_2(3)! - Eigen::Matrix delta_U; - // Bias Jacobian computed in compact form - Eigen::Matrix bias_jacobian; - // Timestamp information - double start_stamp; - double end_stamp; - double dt_total; + IMUDiscretizationMethod discretization; - // Store all measurements to repropagate if needed - std::vector dt_buf; - std::vector acc_buf; - std::vector gyr_buf; + /** + * @brief Returns the continuous-time noise covariance matrix Q_ct + * based on the noise parameters. + */ + Eigen::Matrix continuousTimeNoiseCovariance() const { + Eigen::Matrix Q_ct = + Eigen::Matrix::Identity(); + Q_ct.block<3, 3>(0, 0) *= sigma_gyro_ct * sigma_gyro_ct; + Q_ct.block<3, 3>(3, 3) *= sigma_accel_ct * sigma_accel_ct; + Q_ct.block<3, 3>(6, 6) *= sigma_gyro_bias_ct * sigma_gyro_bias_ct; + Q_ct.block<3, 3>(9, 9) *= sigma_accel_bias_ct * sigma_accel_bias_ct; + return Q_ct; + } +}; - IMUIncrement(const Eigen::Matrix Q_ct, - Eigen::Vector3d init_gyro_bias, Eigen::Vector3d init_accel_bias, - double init_stamp, const Eigen::Vector3d &gravity, - const LieDirection &direction, - const ExtendedPoseRepresentation &pose_rep = - ExtendedPoseRepresentation::SE23); +class IMUIncrement { +public: + /** + * @brief Constructor for IMUIncrement + */ + IMUIncrement(const std::shared_ptr &options, + const Eigen::Vector3d &init_gyro_bias, + const Eigen::Vector3d &init_accel_bias); - // IMUIncrement(){}; + /** + * @brief Propagates the RMI forward in time given te new IMU measurements. + * Updates the mean RMI, covariance, and Jacobians. + */ + void propagate(double dt, const Eigen::Vector3d &gyro, + const Eigen::Vector3d &acc); void reset(double new_start_stamp, const Eigen::Vector3d &new_gyro_bias = Eigen::Vector3d::Zero(), const Eigen::Vector3d &new_accel_bias = Eigen::Vector3d::Zero()); - // Add measurements to buffer and then call propagate - void pushBack(double dt, const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel); - - // Propagate forward the RMI, bias Jacobians, and covariance - void propagate(double dt, const Eigen::Vector3d &gyro, - const Eigen::Vector3d &acc); - Eigen::Matrix getDeltaX(); // Repropagate all relevant quantities from a new initial bias void repropagate(const Eigen::Vector3d &init_gyro_bias, const Eigen::Vector3d &init_accel_bias); -protected: - void symmetrize(); + /** + * @brief Gets the propagated covariance matrix on the RMI + */ + Eigen::Matrix covariance() const { return covariance_; } + + /** + * @brief Gets the mean RMI, an element of SE_2(3) + */ + Eigen::Matrix meanRMI() const { return Upsilon_ij_; } + + // Get subcomponents of the mean RMI + Eigen::Matrix3d delta_Cij() const { return Upsilon_ij_.block<3, 3>(0, 0); } + Eigen::Vector3d delta_vij() const { return Upsilon_ij_.block<3, 1>(3, 0); } + Eigen::Vector3d delta_rij() const { return Upsilon_ij_.block<3, 1>(4, 0); } + /** + * @brief Returns the full Jacobian + */ + Eigen::Matrix jacobian() const { return jacobian_; } + + /** + * @brief returns the bias Jacobian + */ + Eigen::Matrix biasJacobian() const { + return jacobian_.block<9, 6>(0, 9); + } + + // Get the RMI propagation time + double deltaT() const { return end_stamp - start_stamp; } + + double startTime() const { return start_stamp; } + double endTime() const { return end_stamp; } + + std::shared_ptr options() const { return options_; } + + Eigen::Vector3d gyroBias() const { return gyro_bias; } + Eigen::Vector3d accelBias() const { return accel_bias; } + +protected: /** * @brief Propagates forward the covariance and bias Jacobian. */ @@ -91,6 +149,45 @@ class IMUIncrement { const Eigen::Vector3d &r, const Eigen::Vector3d &omega, const Eigen::Vector3d &accel, Eigen::Matrix &A_ct, Eigen::Matrix &L_ct); + + /** + * @brief The options for our IMUIncrement + */ + std::shared_ptr options_; + + // The mean value of the RMI, an element of SE_2(3) + Eigen::Matrix Upsilon_ij_; + + // Covariance and Jacobian that we'll propagate forward at each iteration + Eigen::Matrix covariance_; + Eigen::Matrix jacobian_; + + // Initial estimates gyro and accel bias + Eigen::Vector3d gyro_bias; + Eigen::Vector3d accel_bias; + + // Timestamp information + double start_stamp; + double end_stamp; + + std::vector dt_buf; + std::vector acc_buf; + std::vector gyr_buf; }; +/// Helper functions for preintegration +/** + * @brief Creates the Phi matrix for a given dt and extended pose T + */ +Eigen::Matrix phiMat(double dt, + const Eigen::Matrix &T); + +/// Creates the Upsilon matrix +Eigen::Matrix upsilonMat(double dt, const Eigen::Vector3d &omega, + const Eigen::Vector3d &accel, + IMUDiscretizationMethod discretization); + +/// Creates the Psi matrix +Eigen::Matrix3d psiMat(const Eigen::Vector3d &omega); + } // namespace ceres_nav diff --git a/include/imu/IMUPreintegrationHelper.h b/include/imu/IMUPreintegrationHelper.h index 8af0b29..06f2836 100644 --- a/include/imu/IMUPreintegrationHelper.h +++ b/include/imu/IMUPreintegrationHelper.h @@ -66,16 +66,23 @@ class IMUPreintegrationHelper { const IMUStateHolder &X_j) const; // Gets the covariance of the preintegrated measurement - Eigen::Matrix covariance() const { return rmi.covariance; } + Eigen::Matrix covariance() const { return rmi.covariance(); } - double startStamp() const { return rmi.start_stamp; } - double endStamp() const { return rmi.end_stamp; } + double startStamp() const { return rmi.startTime(); } + double endStamp() const { return rmi.endTime(); } private: const IMUIncrement rmi; bool use_group_jacobians; LieDirection direction; ExtendedPoseRepresentation pose_rep; + + // Helper functions to compute matrices used in Jacobian calculations + Eigen::Matrix + computePhiMatrix(const Eigen::Matrix &T, double dt) const; + Eigen::Matrix computeFMatrix(double dt) const; + Eigen::Matrix + computeGammaMatrix(double dt, const Eigen::Vector3d &gravity) const; }; } // namespace ceres_nav \ No newline at end of file diff --git a/src/imu/IMUHelper.cpp b/src/imu/IMUHelper.cpp deleted file mode 100644 index 8dd24ef..0000000 --- a/src/imu/IMUHelper.cpp +++ /dev/null @@ -1,151 +0,0 @@ -#include "lie/SE23.h" -#include "lie/SO3.h" - -#include "imu/IMUHelper.h" -#include "imu/IMUIncrement.h" - -namespace ceres_nav { - -Eigen::Matrix createGMatrix(const Eigen::Vector3d &gravity, - double dt) { - Eigen::Matrix G = Eigen::Matrix::Identity(); - G.block<3, 1>(0, 3) = dt * gravity; - G.block<3, 1>(0, 4) = -0.5 * dt * dt * gravity; - G(3, 4) = -dt; - return G; -} - -Eigen::Matrix3d createNMatrix(const Eigen::Vector3d &phi_vec) { - double small_angle_tol = 1e-7; - double phi_norm = phi_vec.norm(); - if (phi_norm < small_angle_tol) { - return Eigen::Matrix3d::Identity(); - } else { - Eigen::Vector3d a = phi_vec / phi_norm; - Eigen::Matrix3d a_cross = SO3::cross(a); - double c = (1.0 - cos(phi_norm)) / (phi_norm * phi_norm); - double s = (phi_norm - sin(phi_norm)) / (phi_norm * phi_norm); - Eigen::Matrix3d N = 2 * c * Eigen::Matrix3d::Identity() + - (1 - 2 * c) * (a * a.transpose()) + (2 * s * a_cross); - return N; - } -} - -Eigen::Matrix createUMatrix(const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel, - double dt) { - Eigen::Matrix U_mat = Eigen::Matrix::Identity(); - Eigen::Vector3d phi = omega * dt; - Eigen::Matrix3d O_mat = SO3::expMap(phi); - Eigen::Matrix3d J_left = SO3::leftJacobian(phi); - Eigen::Matrix3d V_mat = createNMatrix(phi); - U_mat.block<3, 3>(0, 0) = O_mat; - U_mat.block<3, 1>(0, 3) = dt * J_left * accel; - U_mat.block<3, 1>(0, 4) = (0.5 * dt * dt) * V_mat * accel; - U_mat(3, 4) = dt; - return U_mat; -} - -Eigen::Matrix inverseIE3(const Eigen::Matrix &X) { - Eigen::Matrix3d R_mat = X.block<3, 3>(0, 0); - double c = X(3, 4); - Eigen::Vector3d a = X.block<3, 1>(0, 3); - Eigen::Vector3d b = X.block<3, 1>(0, 4); - Eigen::Matrix X_inv = Eigen::Matrix::Identity(); - X_inv.block<3, 3>(0, 0) = R_mat.transpose(); - X_inv.block<3, 1>(0, 3) = -R_mat.transpose() * a; - X_inv.block<3, 1>(0, 4) = R_mat.transpose() * (c * a - b); - X_inv(3, 4) = -c; - return X_inv; -} - -Eigen::Matrix createUMatrixInv(const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel, - double dt) { - Eigen::Matrix U_matrix = createUMatrix(omega, accel, dt); - Eigen::Matrix U_inv = inverseIE3(U_matrix); - return U_inv; -} - -Eigen::Matrix createLMatrix(const Eigen::Vector3d &unbiased_gyro, - const Eigen::Vector3d &unbiased_accel, - double dt) { - Eigen::Vector3d a = unbiased_accel; - Eigen::Vector3d om = unbiased_gyro; - Eigen::Vector3d om_dt = om * dt; - Eigen::Matrix3d J_att_inv_times_N = - SO3::leftJacobianInverse(om_dt) * createNMatrix(om_dt); - Eigen::VectorXd xi(9); - xi.segment<3>(0) = dt * om; - xi.segment<3>(3) = dt * a; - xi.segment<3>(6) = (0.5 * dt * dt) * J_att_inv_times_N * a; - Eigen::Matrix J_left = SE23::leftJacobian(-xi); - Eigen::Matrix3d Om = SO3::cross(om_dt); - Eigen::Matrix3d OmOm = Om * Om; - Eigen::Matrix3d A = SO3::cross(a); - Eigen::Matrix Up = Eigen::Matrix::Zero(); - Up.block<6, 6>(0, 0) = Eigen::Matrix::Identity() * dt; - double coeff = -0.5 * (dt * dt / 2.0); - double coeff2 = (1.0 / 360.0) * (dt * dt * dt); - Up.block<3, 3>(6, 0) = - coeff * - (coeff2 * (OmOm * A + Om * (SO3::cross(Om * a) + SO3::cross(OmOm * a))) - - (1.0 / 6.0) * dt * A); - Up.block<3, 3>(6, 3) = (0.5 * dt * dt) * J_att_inv_times_N; - auto L = J_left * Up; - return L; -} - -Eigen::Matrix adjointIE3(const Eigen::Matrix &X) { - Eigen::Matrix3d R_mat = X.block<3, 3>(0, 0); - double c = X(3, 4); - Eigen::Vector3d a = X.block<3, 1>(0, 3); - Eigen::Vector3d b = X.block<3, 1>(0, 4); - Eigen::Matrix adjoint; - adjoint.setZero(); - adjoint.block<3, 3>(0, 0) = R_mat; - adjoint.block<3, 3>(3, 0) = SO3::cross(a) * R_mat; - adjoint.block<3, 3>(3, 3) = R_mat; - - adjoint.block<3, 3>(6, 0) = -SO3::cross(c * a - b) * R_mat; - adjoint.block<3, 3>(6, 3) = -c * R_mat; - adjoint.block<3, 3>(6, 6) = R_mat; - - return adjoint; -} - -// bool preintegrateIMUMeasurements(IMUIncrement &rmi, -// const std::vector &imu_meas_vec) { -// for (int i = 0; i < (imu_meas_vec.size() - 1); i++) { -// double dt = imu_meas_vec[i + 1].stamp - imu_meas_vec[i].stamp; -// IMU cur_meas = imu_meas_vec[i]; -// rmi.propagate(dt, cur_meas.gyro, cur_meas.accel); -// } - -// return true; -// } - -// std::vector getIMUBetweenTimes(const double &stamp_i, -// const double &stamp_j, -// const std::vector &imu_meas_vec) { -// std::vector imu_out; -// for (auto const &imu_meas : imu_meas_vec) { -// if ((imu_meas.stamp >= stamp_i) && (imu_meas.stamp <= stamp_j)) { -// imu_out.push_back(imu_meas); -// } -// } - -// return imu_out; -// } - -// bool preintegrateBetweenTimes(IMUIncrement &rmi, const double &stamp_i, -// const double &stamp_j, -// const std::vector &imu_meas_vec) { -// std::vector imu_to_preintegrate = -// getIMUBetweenTimes(stamp_i, stamp_j, imu_meas_vec); -// preintegrateIMUMeasurements(rmi, imu_to_preintegrate); - -// return true; -// } - -} // namespace ceres_nav \ No newline at end of file diff --git a/src/imu/IMUIncrement.cpp b/src/imu/IMUIncrement.cpp index 57be287..8a5aa8c 100644 --- a/src/imu/IMUIncrement.cpp +++ b/src/imu/IMUIncrement.cpp @@ -1,5 +1,4 @@ #include "imu/IMUIncrement.h" -#include "imu/IMUHelper.h" #include "lie/SE23.h" #include "lie/SO3.h" @@ -12,23 +11,12 @@ namespace ceres_nav { -IMUIncrement::IMUIncrement(Eigen::Matrix Q_ct_, - Eigen::Vector3d init_gyro_bias, - Eigen::Vector3d init_accel_bias, double init_stamp, - const Eigen::Vector3d &gravity_, - const LieDirection &direction_, - const ExtendedPoseRepresentation &pose_rep_) - : Q_ct{Q_ct_}, gyro_bias{init_gyro_bias}, accel_bias{init_accel_bias}, - start_stamp{init_stamp}, end_stamp{init_stamp}, gravity{gravity_}, - direction{direction_}, pose_rep{pose_rep_} { - // Set bias Jacobians to zero - bias_jacobian.setZero(); - - // Set delta_U and Jacobian to identity - delta_U.setIdentity(); - jacobian.setIdentity(); - covariance.setZero(); - dt_total = 0.0; +IMUIncrement::IMUIncrement(const std::shared_ptr &options, + const Eigen::Vector3d &init_gyro_bias, + const Eigen::Vector3d &init_accel_bias) + : options_{options}, gyro_bias{init_gyro_bias}, accel_bias{ + init_accel_bias} { + reset(0.0, init_gyro_bias, init_accel_bias); } void IMUIncrement::reset(double new_start_stamp, @@ -36,79 +24,71 @@ void IMUIncrement::reset(double new_start_stamp, const Eigen::Vector3d &new_accel_bias) { gyro_bias = new_gyro_bias; accel_bias = new_accel_bias; + start_stamp = new_start_stamp; end_stamp = new_start_stamp; - dt_total = 0.0; - delta_U.setIdentity(); - jacobian.setIdentity(); - covariance.setZero(); + + Upsilon_ij_.setIdentity(); + covariance_.setZero(); + jacobian_.setIdentity(); + dt_buf.clear(); gyr_buf.clear(); acc_buf.clear(); - bias_jacobian.setZero(); } -void IMUIncrement::pushBack(double dt, const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel) { +void IMUIncrement::propagate(double dt, const Eigen::Vector3d &omega, + const Eigen::Vector3d &accel) { dt_buf.push_back(dt); gyr_buf.push_back(omega); acc_buf.push_back(accel); - propagate(dt, omega, accel); -} -void IMUIncrement::propagate(double dt, const Eigen::Vector3d &omega, - const Eigen::Vector3d &accel) { - dt_total += dt; end_stamp += dt; + Eigen::Vector3d unbiased_gyro = omega - gyro_bias; Eigen::Vector3d unbiased_accel = accel - accel_bias; - Eigen::Matrix U_mat = - createUMatrix(unbiased_gyro, unbiased_accel, dt); - - // Propagate forward the covariance and bias Jacobian + // Propagate covariance propagateCovarianceAndBiasJacobian(dt, omega, accel); - symmetrize(); - - // Update RMI - delta_U = delta_U * U_mat; + // Propagate mean + Eigen::Matrix Upsilon_ip1 = + upsilonMat(dt, unbiased_gyro, unbiased_accel, options_->discretization); + Eigen::Matrix Phi_dt = phiMat(dt, Upsilon_ij_); + Upsilon_ij_ = Phi_dt * Upsilon_ip1; } void IMUIncrement::propagateCovarianceAndBiasJacobian( double dt, const Eigen::Vector3d &omega, const Eigen::Vector3d &accel) { // Compute continuous-time A matrix - Eigen::Matrix3d C = delta_U.block<3, 3>(0, 0); - Eigen::Vector3d v = delta_U.block<3, 1>(0, 3); - Eigen::Vector3d r = delta_U.block<3, 1>(0, 4); + Eigen::Matrix3d C = Upsilon_ij_.block<3, 3>(0, 0); + Eigen::Vector3d v = Upsilon_ij_.block<3, 1>(0, 3); + Eigen::Vector3d r = Upsilon_ij_.block<3, 1>(0, 4); // Compute the continuous-time A and L matrices based on the // state representation and direction Eigen::Matrix A_ct = Eigen::Matrix::Zero(); Eigen::Matrix L_ct = Eigen::Matrix::Zero(); - if (pose_rep == ExtendedPoseRepresentation::SE23) { + if (options_->pose_rep == ExtendedPoseRepresentation::SE23) { computeContinuousTimeJacobiansSE23(C, v, r, omega, accel, A_ct, L_ct); - } else if (pose_rep == ExtendedPoseRepresentation::Decoupled) { + } else if (options_->pose_rep == ExtendedPoseRepresentation::Decoupled) { // LOG(INFO) << "Decoupled Jacobians..."; computeContinuousTimeJacobiansDecoupled(C, v, r, omega, accel, A_ct, L_ct); - } else { - LOG(FATAL) << "Unknown pose representation: " << static_cast(pose_rep); } + Eigen::Matrix Q_ct = + options_->continuousTimeNoiseCovariance(); Eigen::MatrixXd A_d, Q_d; ceres_nav::discretizeSystem(A_ct, L_ct, Q_ct, dt, A_d, Q_d, - ceres_nav::DiscretizationMethod::TaylorSeries); - Q_d = 0.5 * (Q_d + Q_d.transpose()); + ceres_nav::DiscretizationMethod::TaylorSeries); + // Symmetrize Q_d to avoid numerical issues + Q_d = Q_d.selfadjointView(); // Propagate Jacobian forward and extract bias portion - jacobian = A_d * jacobian; - bias_jacobian = jacobian.block<9, 6>(0, 9); - - // Propagate covariance forward - covariance = A_d * covariance * A_d.transpose() + Q_d; -} + jacobian_ = A_d * jacobian_; -void IMUIncrement::symmetrize() { - covariance = 0.5 * (covariance + covariance.transpose()); + // Propagate covariance forward and symmetrize + covariance_ = A_d * covariance_ * A_d.transpose() + Q_d; + covariance_ = covariance_.selfadjointView(); } void IMUIncrement::computeContinuousTimeJacobiansDecoupled( @@ -119,11 +99,11 @@ void IMUIncrement::computeContinuousTimeJacobiansDecoupled( A_ct.setZero(); L_ct.setZero(); - if (direction == LieDirection::left) { + if (options_->direction == LieDirection::left) { LOG(ERROR) << "Left lie direction for decoupled navigation state " - "representation not yet supported with IMU increment!"; + "representation not yet supported with IMU increment!"; std::exit(EXIT_FAILURE); - } else if (direction == LieDirection::right) { + } else if (options_->direction == LieDirection::right) { Eigen::Vector3d unbiased_gyro = omega - gyro_bias; Eigen::Vector3d unbiased_accel = accel - accel_bias; @@ -147,7 +127,7 @@ void IMUIncrement::computeContinuousTimeJacobiansSE23( Eigen::Matrix &L_ct) { A_ct.setZero(); L_ct.setZero(); - if (direction == LieDirection::left) { + if (options_->direction == LieDirection::left) { A_ct.block<3, 3>(0, 9) = -C; A_ct.block<3, 3>(3, 9) = -SO3::cross(v) * C; A_ct.block<3, 3>(3, 12) = -C; @@ -160,7 +140,7 @@ void IMUIncrement::computeContinuousTimeJacobiansSE23( L_ct.block<3, 3>(6, 0) = SO3::cross(r) * C; L_ct.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); L_ct.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); - } else if (direction == LieDirection::right) { + } else if (options_->direction == LieDirection::right) { // Compute unbiased gyro and accel Eigen::Vector3d unbiased_gyro = omega - gyro_bias; Eigen::Vector3d unbiased_accel = accel - accel_bias; @@ -182,21 +162,66 @@ void IMUIncrement::computeContinuousTimeJacobiansSE23( void IMUIncrement::repropagate(const Eigen::Vector3d &init_gyro_bias, const Eigen::Vector3d &init_accel_bias) { - dt_total = 0.0; end_stamp = start_stamp; gyro_bias = init_gyro_bias; accel_bias = init_accel_bias; - // Set bias Jacobians to zero - bias_jacobian.setZero(); // Set delta_U and Jacobian to identity - delta_U.setIdentity(); - jacobian.setIdentity(); - covariance.setZero(); + Upsilon_ij_.setIdentity(); + jacobian_.setIdentity(); + covariance_.setZero(); // Repropagate using all measurements - for (int i = 0; i < static_cast(dt_buf.size()); i++) { + for (size_t i = 0; i < dt_buf.size(); ++i) { propagate(dt_buf[i], gyr_buf[i], acc_buf[i]); } } + +Eigen::Matrix phiMat(double dt, const Eigen::Matrix &T) { + Eigen::Matrix Phi = Eigen::Matrix::Identity(); + Phi.block<3, 3>(0, 0) = T.block<3, 3>(0, 0); + Phi.block<3, 1>(0, 3) = T.block<3, 1>(0, 3); + Phi.block<3, 1>(0, 4) = T.block<3, 1>(0, 4) + dt * T.block<3, 1>(0, 3); + return Phi; +} + +Eigen::Matrix3d psiMat(const Eigen::Vector3d &omega) { + if (omega.norm() < 1e-8) { + return 0.5 * Eigen::Matrix3d::Identity(); + } else { + double phi = omega.norm(); + Eigen::Matrix3d a_cross = SO3::cross(omega); + double s = (phi - sin(phi)) / (phi * phi * phi); + double c = (phi * phi + 2 * cos(phi) - 2) / (2 * phi * phi * phi * phi); + return 0.5 * Eigen::Matrix3d::Identity() + s * a_cross + + c * a_cross * a_cross; + } +} + +Eigen::Matrix upsilonMat(double dt, const Eigen::Vector3d &omega, + const Eigen::Vector3d &accel, IMUDiscretizationMethod discretization) { + Eigen::Matrix Upsilon = Eigen::Matrix::Identity(); + + if (discretization == + IMUDiscretizationMethod::ConstantAccel) { + LOG(ERROR) + << "Constant Accel discretization not yet implemented in upsilonMat!"; + std::exit(EXIT_FAILURE); + } else if (discretization == + IMUDiscretizationMethod::ConstantMeas) { + Eigen::Matrix3d delta_C = SO3::expMap(omega * dt); + Eigen::Matrix3d Psi_2 = psiMat(omega * dt); + + Upsilon.block<3, 3>(0, 0) = delta_C; + Upsilon.block<3, 1>(0, 3) = dt * SO3::leftJacobian(omega * dt) * accel; + Upsilon.block<3, 1>(0, 4) = dt * dt * Psi_2 * accel; + } + else { + LOG(ERROR) << "Unknown discretization method in upsilonMat!"; + std::exit(EXIT_FAILURE); + } + + return Upsilon; +} + } // namespace ceres_nav \ No newline at end of file diff --git a/src/imu/IMUPreintegrationHelper.cpp b/src/imu/IMUPreintegrationHelper.cpp index 3a28fa9..9a073aa 100644 --- a/src/imu/IMUPreintegrationHelper.cpp +++ b/src/imu/IMUPreintegrationHelper.cpp @@ -5,7 +5,8 @@ namespace ceres_nav { IMUPreintegrationHelper::IMUPreintegrationHelper( const IMUIncrement &imu_increment, bool use_group_jacobians_) : rmi{imu_increment}, use_group_jacobians{use_group_jacobians_}, - direction{imu_increment.direction}, pose_rep{imu_increment.pose_rep} {} + direction{imu_increment.options()->direction}, + pose_rep{imu_increment.options()->pose_rep} {} Eigen::Matrix IMUPreintegrationHelper::computePreintegrationError( @@ -48,23 +49,19 @@ IMUPreintegrationHelper::computePreintegrationError( Eigen::Matrix IMUPreintegrationHelper::getUpdatedRMI(const IMUStateHolder &X_i) const { Eigen::Matrix dbias; - Eigen::Vector3d d_bg = X_i.bias_gyro - rmi.gyro_bias; - Eigen::Vector3d d_ba = X_i.bias_accel - rmi.accel_bias; + Eigen::Vector3d d_bg = X_i.bias_gyro - rmi.gyroBias(); + Eigen::Vector3d d_ba = X_i.bias_accel - rmi.accelBias(); dbias.block<3, 1>(0, 0) = d_bg; dbias.block<3, 1>(3, 0) = d_ba; - Eigen::Matrix delta_X = Eigen::Matrix::Identity(); - delta_X.block<3, 3>(0, 0) = rmi.delta_U.block<3, 3>(0, 0); - delta_X.block<3, 1>(0, 3) = rmi.delta_U.block<3, 1>(0, 3); - delta_X.block<3, 1>(0, 4) = rmi.delta_U.block<3, 1>(0, 4); - + Eigen::Matrix delta_X = rmi.meanRMI(); // If we're using an SE_2(3) representation, perform first-order // bias correct directly on the group if (pose_rep == ExtendedPoseRepresentation::SE23) { if (direction == LieDirection::left) { - return SE23::expMap(rmi.bias_jacobian * dbias) * delta_X; + return SE23::expMap(rmi.biasJacobian() * dbias) * delta_X; } else if (direction == LieDirection::right) { - return delta_X * SE23::expMap(rmi.bias_jacobian * dbias); + return delta_X * SE23::expMap(rmi.biasJacobian() * dbias); } else { std::cout << "WARNING: Unknown Lie direction!" << std::endl; return Eigen::Matrix::Identity(); @@ -82,11 +79,11 @@ IMUPreintegrationHelper::getUpdatedRMI(const IMUStateHolder &X_i) const { Eigen::Vector3d delta_r = delta_X.block<3, 1>(0, 4); // Extract relevant parts of the bias Jacobian - Eigen::Matrix3d dC_dbg = rmi.bias_jacobian.block<3, 3>(0, 0); - - Eigen::Matrix dv_db = rmi.bias_jacobian.block<3, 6>(3, 0); - Eigen::Matrix dr_db = rmi.bias_jacobian.block<3, 6>(6, 0); + Eigen::Matrix bias_jacobian = rmi.biasJacobian(); + Eigen::Matrix3d dC_dbg = bias_jacobian.block<3, 3>(0, 0); + Eigen::Matrix dv_db = bias_jacobian.block<3, 6>(3, 0); + Eigen::Matrix dr_db = bias_jacobian.block<3, 6>(6, 0); Eigen::Vector3d delta_v_updated = delta_v + dv_db * dbias; Eigen::Vector3d delta_r_updated = delta_r + dr_db * dbias; @@ -116,7 +113,7 @@ IMUPreintegrationHelper::getUpdatedRMI(const IMUStateHolder &X_i) const { Eigen::Matrix IMUPreintegrationHelper::predictNavRMI(const IMUStateHolder &X_i, const IMUStateHolder &X_j) const { - Eigen::Vector3d g_a = rmi.gravity; + Eigen::Vector3d g_a = rmi.options()->gravity; Eigen::Matrix3d C_i = X_i.attitude; Eigen::Vector3d v_i = X_i.velocity; Eigen::Vector3d r_i = X_i.position; @@ -125,7 +122,7 @@ IMUPreintegrationHelper::predictNavRMI(const IMUStateHolder &X_i, Eigen::Vector3d v_j = X_j.velocity; Eigen::Vector3d r_j = X_j.position; - double delta_t = rmi.end_stamp - rmi.start_stamp; + double delta_t = rmi.deltaT(); Eigen::Matrix3d delta_C = C_i.transpose() * C_j; Eigen::Vector3d delta_v = C_i.transpose() * (v_j - v_i - g_a * delta_t); Eigen::Vector3d delta_r = C_i.transpose() * (r_j - r_i - v_i * delta_t - @@ -161,89 +158,75 @@ IMUPreintegrationHelper::computeRawJacobians(const IMUStateHolder &X_i, return std::vector>(); } } + + return std::vector>(); } std::vector> IMUPreintegrationHelper::computeRawJacobiansLeftSE23( const IMUStateHolder &X_i, const IMUStateHolder &X_j) const { // Extract individual states - Eigen::Vector3d g_a = rmi.gravity; - Eigen::Matrix3d C_i = X_i.attitude; - Eigen::Vector3d v_i = X_i.velocity; - Eigen::Vector3d r_i = X_i.position; + Eigen::Vector3d g_a = rmi.options()->gravity; Eigen::Vector3d ba_i = X_i.bias_accel; + double delta_t = rmi.deltaT(); - Eigen::Vector3d v_j = X_j.velocity; - Eigen::Vector3d r_j = X_j.position; - - Eigen::Matrix del_Xy_mat = getUpdatedRMI(X_i); - Eigen::Matrix del_Xx_mat = predictNavRMI(X_i, X_j); + // Compute Jacobian of \Delta_X with respect to X_i + Eigen::Matrix D_deltaX_D_Xi = + Eigen::Matrix::Zero(); + Eigen::Matrix T_i = Eigen::Matrix::Identity(); + T_i.block<3, 3>(0, 0) = X_i.attitude; + T_i.block<3, 1>(0, 3) = X_i.velocity; + T_i.block<3, 1>(0, 4) = X_i.position; - // Extract required subcomponents of matrix - Eigen::Matrix del_Xx_v = del_Xx_mat.block<3, 1>(0, 3); - Eigen::Matrix del_Xx_r = del_Xx_mat.block<3, 1>(0, 4); - double delta_t = rmi.end_stamp - rmi.start_stamp; + Eigen::Matrix Phi_Ti_inv = + SE23::inverse(computePhiMatrix(T_i, delta_t)); + Eigen::Matrix d_Upsilon_dTi = + -SE23::adjoint(Phi_Ti_inv) * computeFMatrix(delta_t); // Jacobian of \Delta X^X with respect to X_i - Eigen::Matrix D_delta_xx_Xi = - Eigen::Matrix::Zero(); - D_delta_xx_Xi.block<3, 3>(0, 0) = -C_i.transpose(); - D_delta_xx_Xi.block<3, 3>(3, 0) = C_i.transpose() * SO3::cross(v_i); - D_delta_xx_Xi.block<3, 3>(3, 3) = -C_i.transpose(); - D_delta_xx_Xi.block<3, 3>(6, 0) = - C_i.transpose() * SO3::cross(r_i + v_i * delta_t); - D_delta_xx_Xi.block<3, 3>(6, 3) = -delta_t * C_i.transpose(); - D_delta_xx_Xi.block<3, 3>(6, 6) = -C_i.transpose(); - D_delta_xx_Xi.block<6, 6>(9, 9) = -Eigen::Matrix::Identity(); + D_deltaX_D_Xi.block<9, 9>(0, 0) = d_Upsilon_dTi; + D_deltaX_D_Xi.block<6, 6>(9, 9) = -Eigen::Matrix::Identity(); - // Compute Jacobian of error w.r.t del_Xy + // Jacobian of \Delta X_hat with respect to X_i (due to bias update) + Eigen::Matrix D_deltaXhat_D_Xi = + Eigen::Matrix::Zero(); Eigen::Matrix dbias; - dbias.block<3, 1>(0, 0) = X_i.bias_gyro - rmi.gyro_bias; - dbias.block<3, 1>(3, 0) = X_i.bias_accel - rmi.accel_bias; - Eigen::Matrix bias_jac = rmi.bias_jacobian; + dbias.block<3, 1>(0, 0) = X_i.bias_gyro - rmi.gyroBias(); + dbias.block<3, 1>(3, 0) = X_i.bias_accel - rmi.accelBias(); + Eigen::Matrix bias_jac = rmi.biasJacobian(); Eigen::Matrix tau = bias_jac * dbias; - Eigen::Matrix Ji_X_b = SE23::leftJacobian(bias_jac * dbias) * bias_jac; - Eigen::Matrix xi_i_y = Eigen::Matrix::Zero(); - xi_i_y.block<9, 6>(0, 9) = Ji_X_b; + D_deltaXhat_D_Xi.block<9, 6>(0, 9) = Ji_X_b; // If we want to include the group jacobians, need to compute // the Jacobian of the error with respect to the delta_X^Y // and delta_X^X - Eigen::Matrix De_D_delta_xy = + Eigen::Matrix De_D_delta_Xhat = -Eigen::Matrix::Identity(); - Eigen::Matrix De_D_delta_xx = + Eigen::Matrix De_D_delta_X = Eigen::Matrix::Identity(); if (use_group_jacobians) { + LOG(INFO) << "Using group Jacobians!"; Eigen::Matrix error = computePreintegrationError(X_i, X_j); Eigen::Matrix e_nav = error.block<9, 1>(0, 0); - De_D_delta_xx.block<9, 9>(0, 0) = SE23::leftJacobianInverse(e_nav); - De_D_delta_xy.block<9, 9>(0, 0) = -SE23::rightJacobianInverse(e_nav); + De_D_delta_X.block<9, 9>(0, 0) = SE23::leftJacobianInverse(e_nav); + De_D_delta_Xhat.block<9, 9>(0, 0) = -SE23::rightJacobianInverse(e_nav); } Eigen::Matrix jac_i = - De_D_delta_xy * xi_i_y + De_D_delta_xx * D_delta_xx_Xi; + De_D_delta_X * D_deltaX_D_Xi + De_D_delta_Xhat * D_deltaXhat_D_Xi; // Compute Jacobians of error w.r.t X_j - Eigen::Matrix3d Jj_C_C = C_i.transpose(); - Eigen::Matrix3d Jj_v_C = -C_i.transpose() * SO3::cross(v_j) + - SO3::cross(del_Xx_v) * C_i.transpose(); - Eigen::Matrix3d Jj_v_v = C_i.transpose(); - Eigen::Matrix3d Jj_r_C = -C_i.transpose() * SO3::cross(r_j) + - SO3::cross(del_Xx_r) * C_i.transpose(); - Eigen::Matrix3d Jj_r_r = C_i.transpose(); - // Jacobian of \Delta X^X with respect to X_j - Eigen::Matrix xi_j_x = Eigen::Matrix::Zero(); - xi_j_x.block<3, 3>(0, 0) = Jj_C_C; - xi_j_x.block<3, 3>(3, 0) = Jj_v_C; - xi_j_x.block<3, 3>(3, 3) = Jj_v_v; - xi_j_x.block<3, 3>(6, 0) = Jj_r_C; - xi_j_x.block<3, 3>(6, 6) = Jj_r_r; - xi_j_x.block<6, 6>(9, 9) = Eigen::Matrix::Identity(); - Eigen::Matrix jac_j = De_D_delta_xx * xi_j_x; + Eigen::Matrix D_deltaX_D_Xj = + Eigen::Matrix::Zero(); + Eigen::Matrix gammaInv = + SE23::inverse(computeGammaMatrix(delta_t, g_a)); + D_deltaX_D_Xj.block<9, 9>(0, 0) = SE23::adjoint(Phi_Ti_inv * gammaInv); + D_deltaX_D_Xj.block<6, 6>(9, 9) = Eigen::Matrix::Identity(); + Eigen::Matrix jac_j = De_D_delta_X * D_deltaX_D_Xj; std::vector> raw_jacobians; raw_jacobians.push_back(jac_i); @@ -254,54 +237,45 @@ IMUPreintegrationHelper::computeRawJacobiansLeftSE23( std::vector> IMUPreintegrationHelper::computeRawJacobiansRightSE23( const IMUStateHolder &X_i, const IMUStateHolder &X_j) const { - Eigen::Matrix del_Xx_mat = predictNavRMI(X_i, X_j); + Eigen::Matrix Upsilon_ij_bar = predictNavRMI(X_i, X_j); - // Extract required subcomponents of matrix - Eigen::Matrix3d del_Xx_C = del_Xx_mat.block<3, 3>(0, 0); - Eigen::Vector3d del_Xx_v = del_Xx_mat.block<3, 1>(0, 3); - Eigen::Vector3d del_Xx_r = del_Xx_mat.block<3, 1>(0, 4); - double delta_t = rmi.end_stamp - rmi.start_stamp; + double dt = rmi.deltaT(); + Eigen::Matrix D_deltaX_D_Xi = + Eigen::Matrix::Zero(); + D_deltaX_D_Xi.block<9, 9>(0, 0) = + -SE23::adjoint(SE23::inverse(Upsilon_ij_bar)) * computeFMatrix(dt); + D_deltaX_D_Xi.block<6, 6>(9, 9) = -Eigen::Matrix::Identity(); - // Compute Jacobian of \Delta_X^X with respect to X_i - Eigen::Matrix xi_i_x = Eigen::Matrix::Zero(); - xi_i_x.block<3, 3>(0, 0) = -del_Xx_C.transpose(); - xi_i_x.block<3, 3>(3, 0) = del_Xx_C.transpose() * SO3::cross(del_Xx_v); - xi_i_x.block<3, 3>(3, 3) = -del_Xx_C.transpose(); - xi_i_x.block<3, 3>(6, 0) = del_Xx_C.transpose() * SO3::cross(del_Xx_r); - xi_i_x.block<3, 3>(6, 3) = -delta_t * del_Xx_C.transpose(); - xi_i_x.block<3, 3>(6, 6) = -del_Xx_C.transpose(); - xi_i_x.block<6, 6>(9, 9) = -Eigen::Matrix::Identity(); - - // Compute Jacobian of error with respect to \Delta X^Y + // Jacobian of \Delta_X_hat with respect to X_i (due to bias update) + Eigen::Matrix D_deltaXhat_D_Xi = + Eigen::Matrix::Zero(); Eigen::Matrix dbias; - dbias.block<3, 1>(0, 0) = X_i.bias_gyro - rmi.gyro_bias; - dbias.block<3, 1>(3, 0) = X_i.bias_accel - rmi.accel_bias; - Eigen::Matrix bias_jac = rmi.bias_jacobian; + dbias.block<3, 1>(0, 0) = X_i.bias_gyro - rmi.gyroBias(); + dbias.block<3, 1>(3, 0) = X_i.bias_accel - rmi.accelBias(); + Eigen::Matrix bias_jac = rmi.biasJacobian(); Eigen::Matrix tau = bias_jac * dbias; - Eigen::Matrix Ji_X_b = SE23::rightJacobian(tau) * bias_jac; - - Eigen::Matrix xi_i_y = Eigen::Matrix::Zero(); - xi_i_y.block<9, 6>(0, 9) = Ji_X_b; + D_deltaXhat_D_Xi.block<9, 6>(0, 9) = SE23::rightJacobian(tau) * bias_jac; // Jacobian of \Delta X^X with respect to X_j - Eigen::Matrix jac_j = + Eigen::Matrix D_deltaX_DXj = Eigen::Matrix::Identity(); - Eigen::Matrix De_D_delta_xy = - -Eigen::Matrix::Identity(); - Eigen::Matrix De_D_delta_xx = + // Jacobians of errors with respect to RMI + Eigen::Matrix De_D_DeltaX = + Eigen::Matrix::Identity(); + Eigen::Matrix De_D_DeltaXhat = Eigen::Matrix::Identity(); if (use_group_jacobians) { Eigen::Matrix error = computePreintegrationError(X_i, X_j); Eigen::Matrix e_nav = error.block<9, 1>(0, 0); - De_D_delta_xy.block<9, 9>(0, 0) = -SE23::leftJacobianInverse(e_nav); - De_D_delta_xx.block<9, 9>(0, 0) = SE23::rightJacobianInverse(e_nav); + De_D_DeltaXhat.block<9, 9>(0, 0) = -SE23::leftJacobianInverse(e_nav); + De_D_DeltaX.block<9, 9>(0, 0) = SE23::rightJacobianInverse(e_nav); } Eigen::Matrix jac_i = - De_D_delta_xy * xi_i_y + De_D_delta_xx * xi_i_x; - jac_j = De_D_delta_xx * jac_j; + De_D_DeltaX * D_deltaX_D_Xi + De_D_DeltaXhat * D_deltaXhat_D_Xi; + Eigen::Matrix jac_j = De_D_DeltaX * D_deltaX_DXj; std::vector> raw_jacobians; raw_jacobians.push_back(jac_i); @@ -318,7 +292,7 @@ IMUPreintegrationHelper::computeRawJacobiansRightDecoupled( Eigen::Matrix3d del_Xx_C = del_Xx_mat.block<3, 3>(0, 0); Eigen::Vector3d del_Xx_v = del_Xx_mat.block<3, 1>(0, 3); Eigen::Vector3d del_Xx_r = del_Xx_mat.block<3, 1>(0, 4); - double delta_t = rmi.end_stamp - rmi.start_stamp; + double delta_t = rmi.deltaT(); Eigen::Matrix3d C_i = X_i.attitude; @@ -335,10 +309,11 @@ IMUPreintegrationHelper::computeRawJacobiansRightDecoupled( // Compute Jacobian of \Delta_X^Y with respect to X_i // We need the bias Jacobian here - Eigen::Matrix bias_jac = rmi.bias_jacobian; - Eigen::Matrix D_delta_xy_Xi = Eigen::Matrix::Zero(); + Eigen::Matrix right_jac_part = + Eigen::Matrix::Zero(); + Eigen::Matrix bias_jac = rmi.biasJacobian(); D_delta_xy_Xi.block<9, 6>(0, 9) = bias_jac; // Compute Jacobian of \Delta X^X with respect to X_j @@ -371,4 +346,33 @@ IMUPreintegrationHelper::computeRawJacobiansRightDecoupled( return raw_jacobians; } +Eigen::Matrix +IMUPreintegrationHelper::computePhiMatrix(const Eigen::Matrix &T, + double dt) const { + Eigen::Matrix Phi = Eigen::Matrix::Identity(); + + Eigen::Vector3d v = T.block<3, 1>(0, 3); + Eigen::Vector3d r = T.block<3, 1>(0, 4); + + Phi.block<3, 3>(0, 0) = T.block<3, 3>(0, 0); + Phi.block<3, 1>(0, 3) = T.block<3, 1>(0, 3); + Phi.block<3, 1>(0, 4) = r + dt * v; + return Phi; +} + +Eigen::Matrix +IMUPreintegrationHelper::computeFMatrix(double dt) const { + Eigen::Matrix F = Eigen::Matrix::Identity(); + F.block<3, 3>(6, 3) = Eigen::Matrix3d::Identity() * dt; + return F; +} + +Eigen::Matrix IMUPreintegrationHelper::computeGammaMatrix( + double dt, const Eigen::Vector3d &gravity) const { + Eigen::Matrix Gamma = Eigen::Matrix::Identity(); + Gamma.block<3, 1>(0, 3) = dt * gravity; + Gamma.block<3, 1>(0, 4) = 0.5 * dt * dt * gravity; + return Gamma; +} + } // namespace ceres_nav \ No newline at end of file diff --git a/src/lie/SE3.cpp b/src/lie/SE3.cpp index 4a5ec90..d116729 100644 --- a/src/lie/SE3.cpp +++ b/src/lie/SE3.cpp @@ -2,7 +2,8 @@ #include "lie/SE23.h" #include "lie/SO3.h" #include -#include + +#include namespace ceres_nav { @@ -175,9 +176,10 @@ Eigen::Matrix SE3::minus(const Eigen::Matrix &Y, if (direction == LieDirection::left) { return SE3::logMap(Y * SE3::inverse(X)); } else if (direction == LieDirection::right) { + LOG(INFO) << "Calling right minus"; return SE3::logMap(SE3::inverse(X) * Y); } else { - std::cerr << "Invalid Lie direction" << std::endl; + LOG(ERROR) << "Direction not supported!"; return Eigen::Matrix::Zero(); } } diff --git a/tests/test_factor_graph.cpp b/tests/test_factor_graph.cpp index 2e69cc4..e1dc2e0 100644 --- a/tests/test_factor_graph.cpp +++ b/tests/test_factor_graph.cpp @@ -113,12 +113,16 @@ TEST_CASE("Test MarkovBlanketInfo") { factor_graph.addState(b1_id, b1); // Create and add IMU preintegration factor between X0, b0 and X1, b1 - IMUIncrement rmi0(Eigen::Matrix::Identity(), - Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), 0.0, - Eigen::Vector3d(0, 0, -9.81), LieDirection::right); - IMUIncrement rmi1(Eigen::Matrix::Identity(), - Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), 1.0, - Eigen::Vector3d(0, 0, -9.81), LieDirection::right); + std::shared_ptr preint_options = std::make_shared< + IMUIncrementOptions>(); + preint_options->sigma_gyro_ct = 0.01; + preint_options->sigma_accel_ct = 0.1; + preint_options->sigma_gyro_bias_ct = 0.001; + preint_options->sigma_accel_bias_ct = 0.001; + IMUIncrement rmi0(preint_options, Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero()); + IMUIncrement rmi1(preint_options, Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero()); ceres::CostFunction *preintegration_factor = new IMUPreintegrationFactor(rmi0, false); diff --git a/tests/test_jacobians.cpp b/tests/test_jacobians.cpp index db6f3e5..3d2478b 100644 --- a/tests/test_jacobians.cpp +++ b/tests/test_jacobians.cpp @@ -185,7 +185,7 @@ TEST_CASE("IMUPreintegrationFactor") { Eigen::Matrix Q_ct = Eigen::Matrix::Identity() * 0.01; Eigen::Vector3d init_gyro_bias{0.1, 0.2, 0.3}; - Eigen::Vector3d init_accel_bias{0.1, 0.2, 0.3}; + Eigen::Vector3d init_accel_bias{0.5, 0.7, 0.4}; Eigen::Vector3d gravity(0.0, 0.0, -9.81); int num_imu_meas = 10; @@ -196,7 +196,8 @@ TEST_CASE("IMUPreintegrationFactor") { Eigen::Vector3d r_i = Eigen::Vector3d(1.0, 1.0, 1.0); Eigen::Matrix X_i = SE23::fromComponents(C_i, v_i, r_i); Eigen::Matrix b_i; - b_i << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + b_i << -0.1, -0.2, -0.3, -0.4, -0.5, -0.6; + // b_i << 0.1, 0.2, 0.3, 0.5, 0.7, 0.4; Eigen::Matrix3d C_j = SO3::expMap(Eigen::Vector3d(0.7, 0.5, 0.3)); Eigen::Vector3d v_j = Eigen::Vector3d(0.4, 0.6, 0.76); @@ -206,11 +207,15 @@ TEST_CASE("IMUPreintegrationFactor") { b_j << 0.5, 0.6, 0.2, 0.2, 0.1, 0.4; // Pose representations to test - std::vector rep_types = - {ExtendedPoseRepresentation::SE23, ExtendedPoseRepresentation::Decoupled}; + // std::vector rep_types = + // {ExtendedPoseRepresentation::SE23, + // ExtendedPoseRepresentation::Decoupled}; - std::vector - directions = {LieDirection::left, LieDirection::right}; + std::vector directions = {LieDirection::left, + LieDirection::right}; + + std::vector rep_types = { + ExtendedPoseRepresentation::SE23}; for (auto const &rep_type : rep_types) { for (auto const &direction : directions) { @@ -240,19 +245,29 @@ TEST_CASE("IMUPreintegrationFactor") { parameter_blocks.push_back(b_j_block); // Create an IMU increment and propagate it forward - IMUIncrement imu_increment(Q_ct, init_gyro_bias, init_accel_bias, 0.0, - gravity, direction, rep_type); + LOG(INFO) << "Creating preintegration options..."; + std::shared_ptr preintegration_options = std::make_shared< + IMUIncrementOptions>(); + preintegration_options->sigma_gyro_ct = 0.01; + preintegration_options->sigma_accel_ct = 0.1; + preintegration_options->sigma_gyro_bias_ct = 0.001; + preintegration_options->sigma_accel_bias_ct = 0.001; + preintegration_options->gravity = gravity; + preintegration_options->direction = direction; + preintegration_options->pose_rep = rep_type; + IMUIncrement imu_increment(preintegration_options, init_gyro_bias, + init_accel_bias); for (int i = 0; i < num_imu_meas; ++i) { double dt = 0.01; Eigen::Vector3d omega = Eigen::Vector3d::Random(); Eigen::Vector3d accel = Eigen::Vector3d::Random(); - imu_increment.pushBack(dt, omega, accel); + imu_increment.propagate(dt, omega, accel); } // Create the factor std::shared_ptr factor = - std::make_shared( - imu_increment, use_group_jacobians); + std::make_shared(imu_increment, + use_group_jacobians); // Evaluate the factor with the parameter blocks std::vector analytical_jacobians; @@ -264,13 +279,17 @@ TEST_CASE("IMUPreintegrationFactor") { for (size_t i = 0; i < analytical_jacobians.size(); ++i) { Eigen::MatrixXd difference = analytical_jacobians[i] - numerical_jacobians[i]; - // double norm = difference.norm(); + double norm = difference.norm(); // std::cout << "Jacobian " << i << " norm difference: " << norm // << std::endl; // std::cout << "Jacobian " << i << std::endl; + // std::cout << "Analytical:\n" + // << analytical_jacobians[i] << std::endl; + // std::cout << "Numerical:\n" + // << numerical_jacobians[i] << std::endl; // std::cout << "Difference:\n" << difference << std::endl; } - // REQUIRE(is_correct); + REQUIRE(is_correct); } } } \ No newline at end of file