Skip to content

Simplify preintegration code #20

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
mitchellcohen3 merged 1 commit into from
Dec 3, 2025
Merged

Simplify preintegration code #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
54 changes: 31 additions & 23 deletions examples/gps_imu_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,8 @@ void runSlidingWindowEstimator(
const std::vector<GPSMessage> &gps_data, const IMUState &init_imu_state,
const Eigen::Matrix<double, 15, 15> &init_cov, LieDirection lie_direction,
ExtendedPoseRepresentation state_rep,
const Eigen::Matrix<double, 12, 12> &Q_ct, const Eigen::Matrix3d &R_gps,
const Eigen::Vector3d &gravity, const std::string &est_imu_file,
std::shared_ptr<IMUIncrementOptions> 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;
Expand All @@ -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<double> est_stamps;
Expand All @@ -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,
Expand Down Expand Up @@ -170,8 +171,8 @@ void runFullBatchEstimator(
const std::vector<GPSMessage> &gps_data, const IMUState &init_imu_state,
const Eigen::Matrix<double, 15, 15> &init_cov, LieDirection lie_direction,
ExtendedPoseRepresentation state_rep,
const Eigen::Matrix<double, 12, 12> &Q_ct, const Eigen::Matrix3d &R_gps,
const Eigen::Vector3d &gravity, const std::string &est_imu_file,
std::shared_ptr<IMUIncrementOptions> 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
Expand All @@ -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;
Expand All @@ -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,
Expand Down Expand Up @@ -306,12 +309,6 @@ int main(int argc, const char **argv) {
double sigma_gyro_rw = args["sigma_gyro_random_walk_continuous"].as<double>();
double sigma_accel_rw =
args["sigma_accel_random_walk_continuous"].as<double>();
Eigen::Matrix<double, 12, 12> Q_ct =
Eigen::Matrix<double, 12, 12>::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<double>();
Eigen::Vector3d gravity = Eigen::Vector3d(0.0, 0.0, -gravity_mag);
Expand All @@ -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<IMUIncrementOptions> preint_options =
std::make_shared<IMUIncrementOptions>();
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>();
std::string est_imu_states_file = output_dir + "/optimized_imu_states.txt";
Expand All @@ -342,13 +350,13 @@ int main(int argc, const char **argv) {
Eigen::Matrix<double, 15, 15>::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<int>();
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);
}
Expand Down
4 changes: 2 additions & 2 deletions examples/include/FactorGraphUtils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<StateID> state_ids = {StateID(keys.nav_state_key, start_stamp),
StateID(keys.bias_state_key, start_stamp),
StateID(keys.nav_state_key, end_stamp),
Expand Down
27 changes: 16 additions & 11 deletions examples/include/GPSIMUExampleUtils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
#include <fstream>
#include <sstream>

#include "imu/IMUIncrement.h"
#include "lie/SE23.h"
#include "imu/IMUHelper.h"

#include <glog/logging.h>

Expand Down Expand Up @@ -70,9 +70,7 @@ class IMUState {
nav_state_ = nav_state;
}

void setStamp(double stamp) {
timestamp_ = stamp;
}
void setStamp(double stamp) { timestamp_ = stamp; }

Eigen::Matrix<double, 17, 1> toVector() const {
Eigen::Matrix<double, 17, 1> vec;
Expand Down Expand Up @@ -228,17 +226,24 @@ std::vector<IMUState> 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<double, 5, 5> G = ceres_nav::createGMatrix(gravity, dt);
Eigen::Matrix<double, 5, 5> U =
ceres_nav::createUMatrix(unbiased_gyro, unbiased_accel, dt);
Eigen::Matrix<double, 5, 5> T_k = state.navState();

// Propagation written as the product of three SE_2(3) matrices: Gamma_k * Phi
// (T_k) * Upsilon_k
Eigen::Matrix<double, 5, 5> Gamma_k = Eigen::Matrix<double, 5, 5>::Identity();
Gamma_k.block<3, 1>(0, 3) = dt * gravity;
Gamma_k.block<3, 1>(0, 4) = 0.5 * dt * dt * gravity;

Eigen::Matrix<double, 5, 5> prev_extended_pose = state.navState();
Eigen::Matrix<double, 5, 5> next_extended_pose = G * prev_extended_pose * U;
Eigen::Matrix<double, 5, 5> Phi_k = ceres_nav::phiMat(dt, T_k);
Eigen::Matrix<double, 5, 5> Upsilon_k = ceres_nav::upsilonMat(
dt, unbiased_gyro, unbiased_accel, ceres_nav::IMUDiscretizationMethod::ConstantMeas);
Eigen::Matrix<double, 5, 5> next_extended_pose = Gamma_k * Phi_k * Upsilon_k;

double new_stamp = state.timestamp() + dt;
state.setStamp(new_stamp);
Expand Down
1 change: 1 addition & 0 deletions examples/python/run_gps_imu_fusion.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
40 changes: 0 additions & 40 deletions include/imu/IMUHelper.h
View file
Open in desktop

This file was deleted.

Loading