From efa542bf2525f8c686c403e6f272842b094235e9 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Fri, 19 Nov 2021 15:36:09 -0500 Subject: [PATCH 01/60] add parameters reading from configure --- .../config/rpng_sim/kalibr_imu_chain.yaml | 24 +++++++- ov_msckf/launch/simulation.launch | 28 ++++++---- ov_msckf/src/core/VioManagerOptions.h | 55 +++++++++++++++++++ ov_msckf/src/state/StateOptions.h | 8 +++ 4 files changed, 101 insertions(+), 14 deletions(-) diff --git a/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml index 394cdca13..7a7a40e15 100644 --- a/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml +++ b/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /imu0 time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + R_ItoGyro: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + Ta: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + R_ItoAcc: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + Tg: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 4fa0c3739..9d56eb33a 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -24,8 +24,10 @@ - - + + + + @@ -70,16 +72,18 @@ - - - - - - - - - - + + + + + + + + + + + + diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 347ed1f09..222d588f1 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -209,6 +209,16 @@ struct VioManagerOptions { /// Mask images for each camera std::map masks; + /// Map between imu model, 0: Kalibr model and 1: RPNG model + int imu_model = 0; + + /// imu intrinsics + Eigen::VectorXd imu_x_dw; // the columnwise elements for Dw + Eigen::VectorXd imu_x_da; // the columnwise elements for Da + Eigen::VectorXd imu_x_tg; // the ccolumnwise elements for Tg + Eigen::VectorXd imu_quat_AcctoI; // the JPL quat for R_AcctoI + Eigen::VectorXd imu_quat_GyrotoI; // the JPL quat for R_GyrotoI + /** * @brief This function will load and print all state parameters (e.g. sensor extrinsics) * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -280,6 +290,51 @@ struct VioManagerOptions { masks.insert({i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)}); } } + // IMU model + std::string imu_model_str = "kalibr"; + parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); + if(imu_model_str == "kalibr") imu_model = 0; + else if(imu_model_str == "rpng") imu_model = 1; + else { + PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); + } + + // IMU intrinsics + Eigen::Matrix3d Tw = Eigen::Matrix3d::Identity(); + parser->parse_external("relative_config_imu", "imu0", "Tw", Tw); + Eigen::Matrix3d Ta = Eigen::Matrix3d::Identity(); + parser->parse_external("relative_config_imu", "imu0", "Ta", Ta); + Eigen::Matrix3d R_ItoAcc = Eigen::Matrix3d::Identity(); + parser->parse_external("relative_config_imu", "imu0", "R_ItoAcc", R_ItoAcc); + Eigen::Matrix3d R_ItoGyro = Eigen::Matrix3d::Identity(); + parser->parse_external("relative_config_imu", "imu0", "R_ItoGyro", R_ItoGyro); + Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); + parser->parse_external("relative_config_imu", "imu0", "Tg", Tg); + + + // generate the parameters we need + Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d R_AcctoI = R_ItoAcc.transpose(); + Eigen::Matrix3d R_GyrotoI = R_ItoGyro.transpose(); + + // store these parameters + imu_x_da.resize(6,1); + imu_x_dw.resize(6,1); + imu_quat_AcctoI.resize(4,1); + imu_quat_GyrotoI.resize(4,1); + imu_x_tg.resize(9,1); + if(imu_model == 0){ + imu_x_dw << Dw.block<3,1>(0,0), Dw.block<2,1>(1,1), Dw(2,2); + imu_x_da << Da.block<3,1>(0,0), Da.block<2,1>(1,1), Da(2,2); + }else{ + imu_x_dw << Dw(0,0), Dw.block<2,1>(0,1), Dw.block<3,1>(0,2); + imu_x_da << Da(0,0), Da.block<2,1>(0,1), Da.block<3,1>(0,2); + } + imu_quat_GyrotoI = ov_core::rot_2_quat(R_GyrotoI); + imu_quat_AcctoI = ov_core::rot_2_quat(R_AcctoI); + imu_x_tg << Tg.block<3,1>(0,0), Tg.block<3,1>(0,1), Tg.block<3,1>(0,2); + } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 530fb8ba9..b3114231e 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -53,6 +53,12 @@ struct StateOptions { /// Bool to determine whether or not to calibrate camera to IMU time offset bool do_calib_camera_timeoffset = false; + /// Bool to determine whether or not to calibrate the IMU intrinsics + bool do_calib_imu_intrinsics = false; + + /// Bool to determine whether or not to calibrate the Gravity sensitivity + bool do_calib_imu_g_sensitivity = false; + /// Max clone size of sliding window int max_clone_size = 11; @@ -89,6 +95,8 @@ struct StateOptions { parser->parse_config("calib_cam_extrinsics", do_calib_camera_pose); parser->parse_config("calib_cam_intrinsics", do_calib_camera_intrinsics); parser->parse_config("calib_cam_timeoffset", do_calib_camera_timeoffset); + parser->parse_config("calib_imu_intrinsics", do_calib_imu_intrinsics); + parser->parse_config("calib_imu_g_sensitivity", do_calib_imu_g_sensitivity); parser->parse_config("max_clones", max_clone_size); parser->parse_config("max_slam", max_slam_features); parser->parse_config("max_slam_in_update", max_slam_in_update); From 97827c530146b59667c80ae15f7ff26315e4ba58 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Fri, 19 Nov 2021 16:24:55 -0500 Subject: [PATCH 02/60] add the IMU configure --- ov_core/src/utils/sensor_data.h | 110 ++++++++++++++++++++ ov_msckf/src/core/VioManagerOptions.h | 55 ++++------ ov_msckf/src/sim/Simulator.cpp | 24 ++--- ov_msckf/src/state/Propagator.cpp | 8 +- ov_msckf/src/state/Propagator.h | 49 ++------- ov_msckf/src/state/StateOptions.h | 3 + ov_msckf/src/update/UpdaterZeroVelocity.cpp | 10 +- ov_msckf/src/update/UpdaterZeroVelocity.h | 14 +-- 8 files changed, 167 insertions(+), 106 deletions(-) diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index 8aeeb2b0f..e65668e06 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -25,6 +25,7 @@ #include #include #include +#include "utils/quat_ops.h" namespace ov_core { @@ -76,6 +77,115 @@ struct CameraData { } }; +/** + * @brief Struct of our imu noise parameters + */ +struct ImuConfig { + + /// imu model, 0: Kalibr model and 1: RPNG model + int imu_model = 0; + + /// imu intrinsics + Eigen::Matrix imu_x_dw; // the columnwise elements for Dw + Eigen::Matrix imu_x_da; // the columnwise elements for Da + Eigen::Matrix imu_x_tg; // the ccolumnwise elements for Tg + Eigen::Matrix imu_quat_AcctoI; // the JPL quat for R_AcctoI + Eigen::Matrix imu_quat_GyrotoI; // the JPL quat for R_GyrotoI + + /// Gyroscope white noise (rad/s/sqrt(hz)) + double sigma_w = 1.6968e-04; + + /// Gyroscope white noise covariance + double sigma_w_2 = pow(1.6968e-04, 2); + + /// Gyroscope random walk (rad/s^2/sqrt(hz)) + double sigma_wb = 1.9393e-05; + + /// Gyroscope random walk covariance + double sigma_wb_2 = pow(1.9393e-05, 2); + + /// Accelerometer white noise (m/s^2/sqrt(hz)) + double sigma_a = 2.0000e-3; + + /// Accelerometer white noise covariance + double sigma_a_2 = pow(2.0000e-3, 2); + + /// Accelerometer random walk (m/s^3/sqrt(hz)) + double sigma_ab = 3.0000e-03; + + /// Accelerometer random walk covariance + double sigma_ab_2 = pow(3.0000e-03, 2); + + /// Nice print function of what parameters we have loaded + void print() { + PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); + } + + /// get the IMU Dw + Eigen::Matrix3d Dw(){ + Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); + if(imu_model == 0){ + Dw << + imu_x_dw(0), 0, 0, + imu_x_dw(1), imu_x_dw(3), 0, + imu_x_dw(2), imu_x_dw(4), imu_x_dw(5); + }else{ + Dw << + imu_x_dw(0), imu_x_dw(1), imu_x_dw(3), + 0, imu_x_dw(2), imu_x_dw(4), + 0, 0, imu_x_dw(5); + } + return Dw; + } + + /// get the IMU Da + Eigen::Matrix3d Da(){ + Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); + if(imu_model == 0){ + Da << + imu_x_da(0), 0, 0, + imu_x_da(1), imu_x_da(3), 0, + imu_x_da(2), imu_x_da(4), imu_x_da(5); + }else{ + Da << + imu_x_da(0), imu_x_da(1), imu_x_da(3), + 0, imu_x_da(2), imu_x_da(4), + 0, 0, imu_x_da(5); + } + return Da; + } + + /// get the IMU Tg + Eigen::Matrix3d Tg(){ + Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); + Tg << imu_x_tg(0), imu_x_tg(3), imu_x_tg(6), + imu_x_tg(1), imu_x_tg(4), imu_x_tg(7), + imu_x_tg(2), imu_x_tg(5), imu_x_tg(8); + return Tg; + } + + /// get the R_AcctoI + Eigen::Matrix3d R_AcctoI(){ + return ov_core::quat_2_Rot(imu_quat_AcctoI); + } + + /// get the R_GyrotoI + Eigen::Matrix3d R_GyrotoI(){ + return ov_core::quat_2_Rot(imu_quat_GyrotoI); + } + + + + +}; + + + + + } // namespace ov_core #endif // OV_CORE_SENSOR_DATA_H \ No newline at end of file diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 222d588f1..bf72a1da2 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -128,7 +128,7 @@ struct VioManagerOptions { // NOISE / CHI2 ============================ /// IMU noise (gyroscope and accelerometer) - Propagator::NoiseManager imu_noises; + ov_core::ImuConfig imu_config; /// Update options for MSCKF features (pixel noise and chi2 multiplier) UpdaterOptions msckf_options; @@ -151,16 +151,16 @@ struct VioManagerOptions { void print_and_load_noise(const std::shared_ptr &parser = nullptr) { PRINT_DEBUG("NOISE PARAMETERS:\n"); if (parser != nullptr) { - parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", imu_noises.sigma_w); - parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_noises.sigma_wb); - parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_noises.sigma_a); - parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_noises.sigma_ab); - imu_noises.sigma_w_2 = std::pow(imu_noises.sigma_w, 2); - imu_noises.sigma_wb_2 = std::pow(imu_noises.sigma_wb, 2); - imu_noises.sigma_a_2 = std::pow(imu_noises.sigma_a, 2); - imu_noises.sigma_ab_2 = std::pow(imu_noises.sigma_ab, 2); + parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", imu_config.sigma_w); + parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_config.sigma_wb); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_config.sigma_a); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_config.sigma_ab); + imu_config.sigma_w_2 = std::pow(imu_config.sigma_w, 2); + imu_config.sigma_wb_2 = std::pow(imu_config.sigma_wb, 2); + imu_config.sigma_a_2 = std::pow(imu_config.sigma_a, 2); + imu_config.sigma_ab_2 = std::pow(imu_config.sigma_ab, 2); } - imu_noises.print(); + imu_config.print(); if (parser != nullptr) { parser->parse_config("up_msckf_sigma_px", msckf_options.sigma_pix); parser->parse_config("up_msckf_chi2_multipler", msckf_options.chi2_multipler); @@ -209,16 +209,6 @@ struct VioManagerOptions { /// Mask images for each camera std::map masks; - /// Map between imu model, 0: Kalibr model and 1: RPNG model - int imu_model = 0; - - /// imu intrinsics - Eigen::VectorXd imu_x_dw; // the columnwise elements for Dw - Eigen::VectorXd imu_x_da; // the columnwise elements for Da - Eigen::VectorXd imu_x_tg; // the ccolumnwise elements for Tg - Eigen::VectorXd imu_quat_AcctoI; // the JPL quat for R_AcctoI - Eigen::VectorXd imu_quat_GyrotoI; // the JPL quat for R_GyrotoI - /** * @brief This function will load and print all state parameters (e.g. sensor extrinsics) * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -293,8 +283,8 @@ struct VioManagerOptions { // IMU model std::string imu_model_str = "kalibr"; parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); - if(imu_model_str == "kalibr") imu_model = 0; - else if(imu_model_str == "rpng") imu_model = 1; + if(imu_model_str == "kalibr") imu_config.imu_model = 0; + else if(imu_model_str == "rpng") imu_config.imu_model = 1; else { PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); } @@ -319,21 +309,16 @@ struct VioManagerOptions { Eigen::Matrix3d R_GyrotoI = R_ItoGyro.transpose(); // store these parameters - imu_x_da.resize(6,1); - imu_x_dw.resize(6,1); - imu_quat_AcctoI.resize(4,1); - imu_quat_GyrotoI.resize(4,1); - imu_x_tg.resize(9,1); - if(imu_model == 0){ - imu_x_dw << Dw.block<3,1>(0,0), Dw.block<2,1>(1,1), Dw(2,2); - imu_x_da << Da.block<3,1>(0,0), Da.block<2,1>(1,1), Da(2,2); + if(imu_config.imu_model == 0){ + imu_config.imu_x_dw << Dw.block<3,1>(0,0), Dw.block<2,1>(1,1), Dw(2,2); + imu_config.imu_x_da << Da.block<3,1>(0,0), Da.block<2,1>(1,1), Da(2,2); }else{ - imu_x_dw << Dw(0,0), Dw.block<2,1>(0,1), Dw.block<3,1>(0,2); - imu_x_da << Da(0,0), Da.block<2,1>(0,1), Da.block<3,1>(0,2); + imu_config.imu_x_dw << Dw(0,0), Dw.block<2,1>(0,1), Dw.block<3,1>(0,2); + imu_config.imu_x_da << Da(0,0), Da.block<2,1>(0,1), Da.block<3,1>(0,2); } - imu_quat_GyrotoI = ov_core::rot_2_quat(R_GyrotoI); - imu_quat_AcctoI = ov_core::rot_2_quat(R_AcctoI); - imu_x_tg << Tg.block<3,1>(0,0), Tg.block<3,1>(0,1), Tg.block<3,1>(0,2); + imu_config.imu_quat_GyrotoI = ov_core::rot_2_quat(R_GyrotoI); + imu_config.imu_quat_AcctoI = ov_core::rot_2_quat(R_AcctoI); + imu_config.imu_x_tg << Tg.block<3,1>(0,0), Tg.block<3,1>(0,1), Tg.block<3,1>(0,2); } PRINT_DEBUG("STATE PARAMETERS:\n"); diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 3a0c4a4dc..3f6c96607 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -307,20 +307,20 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto // Now add noise to these measurements double dt = 1.0 / params.sim_freq_imu; std::normal_distribution w(0, 1); - wm(0) = omega_inI(0) + true_bias_gyro(0) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(1) = omega_inI(1) + true_bias_gyro(1) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(2) = omega_inI(2) + true_bias_gyro(2) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - am(0) = accel_inI(0) + true_bias_accel(0) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(1) = accel_inI(1) + true_bias_accel(1) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(2) = accel_inI(2) + true_bias_accel(2) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + wm(0) = omega_inI(0) + true_bias_gyro(0) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(1) = omega_inI(1) + true_bias_gyro(1) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(2) = omega_inI(2) + true_bias_gyro(2) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + am(0) = accel_inI(0) + true_bias_accel(0) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(1) = accel_inI(1) + true_bias_accel(1) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(2) = accel_inI(2) + true_bias_accel(2) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); // Move the biases forward in time - true_bias_gyro(0) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_gyro(1) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_gyro(2) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(0) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(1) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(2) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(0) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(1) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(2) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(0) += params.imu_config.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(1) += params.imu_config.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(2) += params.imu_config.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); // Append the current true bias to our history hist_true_bias_time.push_back(timestamp_last_imu); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 720a03387..dcf3210d5 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -391,10 +391,10 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report Eigen::Matrix Qc = Eigen::Matrix::Zero(); - Qc.block(0, 0, 3, 3) = _noises.sigma_w_2 / dt * Eigen::Matrix::Identity(); - Qc.block(3, 3, 3, 3) = _noises.sigma_a_2 / dt * Eigen::Matrix::Identity(); - Qc.block(6, 6, 3, 3) = _noises.sigma_wb_2 * dt * Eigen::Matrix::Identity(); - Qc.block(9, 9, 3, 3) = _noises.sigma_ab_2 * dt * Eigen::Matrix::Identity(); + Qc.block(0, 0, 3, 3) = _imu_config.sigma_w_2 / dt * Eigen::Matrix::Identity(); + Qc.block(3, 3, 3, 3) = _imu_config.sigma_a_2 / dt * Eigen::Matrix::Identity(); + Qc.block(6, 6, 3, 3) = _imu_config.sigma_wb_2 * dt * Eigen::Matrix::Identity(); + Qc.block(9, 9, 3, 3) = _imu_config.sigma_ab_2 * dt * Eigen::Matrix::Identity(); // Compute the noise injected into the state over the interval Qd = G * Qc * G.transpose(); diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index fecb01856..6df21e920 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -39,54 +39,17 @@ namespace ov_msckf { class Propagator { public: - /** - * @brief Struct of our imu noise parameters - */ - struct NoiseManager { - - /// Gyroscope white noise (rad/s/sqrt(hz)) - double sigma_w = 1.6968e-04; - - /// Gyroscope white noise covariance - double sigma_w_2 = pow(1.6968e-04, 2); - - /// Gyroscope random walk (rad/s^2/sqrt(hz)) - double sigma_wb = 1.9393e-05; - - /// Gyroscope random walk covariance - double sigma_wb_2 = pow(1.9393e-05, 2); - - /// Accelerometer white noise (m/s^2/sqrt(hz)) - double sigma_a = 2.0000e-3; - - /// Accelerometer white noise covariance - double sigma_a_2 = pow(2.0000e-3, 2); - - /// Accelerometer random walk (m/s^3/sqrt(hz)) - double sigma_ab = 3.0000e-03; - - /// Accelerometer random walk covariance - double sigma_ab_2 = pow(3.0000e-03, 2); - - /// Nice print function of what parameters we have loaded - void print() { - PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); - PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); - PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); - PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); - } - }; /** * @brief Default constructor * @param noises imu noise characteristics (continuous time) * @param gravity_mag Global gravity magnitude of the system (normally 9.81) */ - Propagator(NoiseManager noises, double gravity_mag) : _noises(noises) { - _noises.sigma_w_2 = std::pow(_noises.sigma_w, 2); - _noises.sigma_a_2 = std::pow(_noises.sigma_a, 2); - _noises.sigma_wb_2 = std::pow(_noises.sigma_wb, 2); - _noises.sigma_ab_2 = std::pow(_noises.sigma_ab, 2); + Propagator( ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { + _imu_config.sigma_w_2 = std::pow(_imu_config.sigma_w, 2); + _imu_config.sigma_a_2 = std::pow(_imu_config.sigma_a, 2); + _imu_config.sigma_wb_2 = std::pow(_imu_config.sigma_wb, 2); + _imu_config.sigma_ab_2 = std::pow(_imu_config.sigma_ab, 2); last_prop_time_offset = 0.0; _gravity << 0.0, 0.0, gravity_mag; } @@ -262,7 +225,7 @@ class Propagator { Eigen::Vector3d &new_p); /// Container for the noise values - NoiseManager _noises; + ov_core::ImuConfig _imu_config; /// Our history of IMU messages (time, angular, linear) std::vector imu_data; diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index b3114231e..c07ca0bc3 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -59,6 +59,9 @@ struct StateOptions { /// Bool to determine whether or not to calibrate the Gravity sensitivity bool do_calib_imu_g_sensitivity = false; + /// Indicator to use which model, 0: kalibr and 1: rpng + int imu_mode = 0; + /// Max clone size of sliding window int max_clone_size = 11; diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 21bab94da..611219837 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -127,11 +127,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Measurement noise (convert from continuous to discrete) // Note the dt time might be different if we have "cut" any imu measurements - R.block(6 * i + 0, 6 * i + 0, 3, 3) *= _noises.sigma_w_2 / dt; + R.block(6 * i + 0, 6 * i + 0, 3, 3) *= _imu_config.sigma_w_2 / dt; if (!integrated_accel_constraint) { - R.block(6 * i + 3, 6 * i + 3, 3, 3) *= _noises.sigma_a_2 / dt; + R.block(6 * i + 3, 6 * i + 3, 3, 3) *= _imu_config.sigma_a_2 / dt; } else { - R.block(6 * i + 3, 6 * i + 3, 3, 3) *= _noises.sigma_a_2 * dt; + R.block(6 * i + 3, 6 * i + 3, 3, 3) *= _imu_config.sigma_a_2 * dt; } dt_summed += dt; } @@ -143,8 +143,8 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Next propagate the biases forward in time // NOTE: G*Qd*G^t = dt*Qd*dt = dt*Qc Eigen::MatrixXd Q_bias = Eigen::MatrixXd::Identity(6, 6); - Q_bias.block(0, 0, 3, 3) *= dt_summed * _noises.sigma_wb; - Q_bias.block(3, 3, 3, 3) *= dt_summed * _noises.sigma_ab; + Q_bias.block(0, 0, 3, 3) *= dt_summed * _imu_config.sigma_wb; + Q_bias.block(3, 3, 3, 3) *= dt_summed * _imu_config.sigma_ab; // Chi2 distance check // NOTE: we also append the propagation we "would do before the update" if this was to be accepted diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index ecd81cc88..111b864d3 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -63,20 +63,20 @@ class UpdaterZeroVelocity { * @param zupt_noise_multiplier Multiplier of our IMU noise matrix (default should be 1.0) * @param zupt_max_disparity Max disparity we should consider to do a update with */ - UpdaterZeroVelocity(UpdaterOptions &options, Propagator::NoiseManager &noises, std::shared_ptr db, + UpdaterZeroVelocity(UpdaterOptions &options, ov_core::ImuConfig &imu_config, std::shared_ptr db, std::shared_ptr prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, double zupt_max_disparity) - : _options(options), _noises(noises), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), + : _options(options), _imu_config(imu_config), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), _zupt_noise_multiplier(zupt_noise_multiplier), _zupt_max_disparity(zupt_max_disparity) { // Gravity _gravity << 0.0, 0.0, gravity_mag; // Save our raw pixel noise squared - _noises.sigma_w_2 = std::pow(_noises.sigma_w, 2); - _noises.sigma_a_2 = std::pow(_noises.sigma_a, 2); - _noises.sigma_wb_2 = std::pow(_noises.sigma_wb, 2); - _noises.sigma_ab_2 = std::pow(_noises.sigma_ab, 2); + _imu_config.sigma_w_2 = std::pow(_imu_config.sigma_w, 2); + _imu_config.sigma_a_2 = std::pow(_imu_config.sigma_a, 2); + _imu_config.sigma_wb_2 = std::pow(_imu_config.sigma_wb, 2); + _imu_config.sigma_ab_2 = std::pow(_imu_config.sigma_ab, 2); // Initialize the chi squared test table with confidence level 0.95 // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 @@ -126,7 +126,7 @@ class UpdaterZeroVelocity { UpdaterOptions _options; /// Container for the imu noise values - Propagator::NoiseManager _noises; + ov_core::ImuConfig _imu_config; /// Feature tracker database with all features in it std::shared_ptr _db; From e3f7af017da7e3a110eb7a69fba9b1a151192dd3 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Fri, 19 Nov 2021 16:44:54 -0500 Subject: [PATCH 03/60] fix imu_config --- ov_msckf/src/core/VioManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index c533a08fe..40fc8996d 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -123,7 +123,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { } // Initialize our state propagator - propagator = std::make_shared(params.imu_noises, params.gravity_mag); + propagator = std::make_shared(params.imu_config, params.gravity_mag); // Our state initialize initializer = std::make_shared(params.init_options, trackFEATS->get_feature_database()); @@ -134,7 +134,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // If we are using zero velocity updates, then create the updater if (params.try_zupt) { - updaterZUPT = std::make_shared(params.zupt_options, params.imu_noises, trackFEATS->get_feature_database(), + updaterZUPT = std::make_shared(params.zupt_options, params.imu_config, trackFEATS->get_feature_database(), propagator, params.gravity_mag, params.zupt_max_velocity, params.zupt_noise_multiplier, params.zupt_max_disparity); } From 7460522f97fdcad0e8ebcf51a80ddc9c5ff7e953 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Mon, 13 Dec 2021 21:46:47 -0800 Subject: [PATCH 04/60] add the imu intrinsic calibration --- ov_core/src/utils/sensor_data.h | 17 + .../config/euroc_mav/estimator_config.yaml | 3 +- .../config/rpng_aruco/estimator_config.yaml | 3 +- .../rpng_ironsides/estimator_config.yaml | 3 +- .../config/rpng_sim/estimator_config.yaml | 3 +- ov_msckf/config/tum_vi/estimator_config.yaml | 3 +- .../uzhfpv_indoor/estimator_config.yaml | 3 +- .../uzhfpv_indoor_45/estimator_config.yaml | 3 +- ov_msckf/src/core/VioManager.cpp | 9 + ov_msckf/src/ros/ROS1Visualizer.cpp | 6 +- ov_msckf/src/ros/ROS2Visualizer.cpp | 6 +- ov_msckf/src/ros/RosVisualizerHelper.h | 145 ++++ ov_msckf/src/sim/Simulator.cpp | 23 +- ov_msckf/src/state/Propagator.cpp | 672 ++++++++++++++++-- ov_msckf/src/state/Propagator.h | 45 +- ov_msckf/src/state/State.cpp | 101 +++ ov_msckf/src/state/State.h | 30 + ov_msckf/src/state/StateHelper.h | 1 + ov_msckf/src/state/StateOptions.h | 18 +- 19 files changed, 993 insertions(+), 101 deletions(-) diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index e65668e06..5ec71eada 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -158,6 +158,15 @@ struct ImuConfig { return Da; } + /// get the IMU Tw + Eigen::Matrix3d Tw() { + return Dw().inverse(); + } + + Eigen::Matrix3d Ta() { + return Da().inverse(); + } + /// get the IMU Tg Eigen::Matrix3d Tg(){ Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); @@ -172,11 +181,19 @@ struct ImuConfig { return ov_core::quat_2_Rot(imu_quat_AcctoI); } + Eigen::Matrix3d R_ItoAcc(){ + return R_AcctoI().transpose(); + } + /// get the R_GyrotoI Eigen::Matrix3d R_GyrotoI(){ return ov_core::quat_2_Rot(imu_quat_GyrotoI); } + Eigen::Matrix3d R_ItoGyro(){ + return R_GyrotoI().transpose(); + } + diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index f73586437..31d72b158 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/ov_msckf/config/rpng_aruco/estimator_config.yaml index 57738acf8..48ad03e9c 100644 --- a/ov_msckf/config/rpng_aruco/estimator_config.yaml +++ b/ov_msckf/config/rpng_aruco/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/ov_msckf/config/rpng_ironsides/estimator_config.yaml index 9bc2908d9..36bfe2759 100644 --- a/ov_msckf/config/rpng_ironsides/estimator_config.yaml +++ b/ov_msckf/config/rpng_ironsides/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index ffed39420..de78b4e22 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml index 53a6fbaf6..d122789b7 100644 --- a/ov_msckf/config/tum_vi/estimator_config.yaml +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml index 1adcde617..b6ce6db3a 100644 --- a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml index 1adcde617..b6ce6db3a 100644 --- a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml @@ -4,7 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 40fc8996d..7a538678a 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -47,6 +47,15 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // Create the state!! state = std::make_shared(params.state_options); + // set the IMU intrinsics + + state->_imu_x_dw->set_value(params.imu_config.imu_x_dw); + state->_imu_x_da->set_value(params.imu_config.imu_x_da); + state->_imu_x_tg->set_value(params.imu_config.imu_x_tg); + state->_imu_quat_gyrotoI->set_value(params.imu_config.imu_quat_GyrotoI); + state->_imu_quat_acctoI->set_value(params.imu_config.imu_quat_AcctoI); + + // Timeoffset from camera to IMU Eigen::VectorXd temp_camimu_dt; temp_camimu_dt.resize(1); diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index f836cd11a..469ac1869 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -105,15 +105,15 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // Open the files of_state_est.open(filepath_est.c_str()); of_state_std.open(filepath_std.c_str()); - of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; - of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; + of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; // Groundtruth if we are simulating if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); of_state_gt.open(filepath_gt.c_str()); - of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; } } } diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index f70e44c70..617335761 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -109,15 +109,15 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p // Open the files of_state_est.open(filepath_est.c_str()); of_state_std.open(filepath_std.c_str()); - of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; - of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; + of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; // Groundtruth if we are simulating if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); of_state_gt.open(filepath_gt.c_str()); - of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; } } } diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 45fc7c6fc..e1c2dc081 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -281,6 +281,41 @@ class RosVisualizerHelper { << sim->get_true_parameters().camera_extrinsics.at(i)(6) << " "; } + + // Get the base IMU information + of_state_gt.precision(0); + of_state_gt << sim->get_true_parameters().imu_config.imu_model << " "; + of_state_gt.precision(8); + + of_state_gt << sim->get_true_parameters().imu_config.imu_x_dw(0) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(1) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(2) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(3) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(4) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(5) << " "; + of_state_gt << sim->get_true_parameters().imu_config.imu_x_da(0) << " " + << sim->get_true_parameters().imu_config.imu_x_da(1) << " " + << sim->get_true_parameters().imu_config.imu_x_da(2) << " " + << sim->get_true_parameters().imu_config.imu_x_da(3) << " " + << sim->get_true_parameters().imu_config.imu_x_da(4) << " " + << sim->get_true_parameters().imu_config.imu_x_da(5) << " "; + of_state_gt << sim->get_true_parameters().imu_config.imu_x_tg(0) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(1) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(2) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(3) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(4) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(5) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(6) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(7) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(8) << " "; + of_state_gt << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(0) << " " + << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(1) << " " + << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(2) << " " + << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(3) << " "; + of_state_gt << sim->get_true_parameters().imu_config.imu_quat_AcctoI(0) << " " + << sim->get_true_parameters().imu_config.imu_quat_AcctoI(1) << " " + << sim->get_true_parameters().imu_config.imu_quat_AcctoI(2) << " " + << sim->get_true_parameters().imu_config.imu_quat_AcctoI(3) << " "; // New line of_state_gt << endl; } @@ -373,6 +408,116 @@ class RosVisualizerHelper { } } + + /// base IMU parameters + of_state_est.precision(0); + of_state_est << state->_options.imu_model << " "; + of_state_est.precision(8); + + of_state_std.precision(0); + of_state_std << state->_options.imu_model << " "; + of_state_std.precision(8); + + // imu intrinsics + // dw + of_state_est << state->_imu_x_dw->value()(0) << " " + << state->_imu_x_dw->value()(1) << " " + << state->_imu_x_dw->value()(2) << " " + << state->_imu_x_dw->value()(3) << " " + << state->_imu_x_dw->value()(4) << " " + << state->_imu_x_dw->value()(5) << " "; + if (state->_options.do_calib_imu_intrinsics) { + int index_dw = state->_imu_x_dw->id(); + of_state_std << std::sqrt(cov(index_dw + 0, index_dw + 0)) << " " + << std::sqrt(cov(index_dw + 1, index_dw + 1)) << " " + << std::sqrt(cov(index_dw + 2, index_dw + 2)) << " " + << std::sqrt(cov(index_dw + 3, index_dw + 3)) << " " + << std::sqrt(cov(index_dw + 4, index_dw + 4)) << " " + << std::sqrt(cov(index_dw + 5, index_dw + 5)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + + // da + of_state_est << state->_imu_x_da->value()(0) << " " + << state->_imu_x_da->value()(1) << " " + << state->_imu_x_da->value()(2) << " " + << state->_imu_x_da->value()(3) << " " + << state->_imu_x_da->value()(4) << " " + << state->_imu_x_da->value()(5) << " "; + if (state->_options.do_calib_imu_intrinsics) { + int index_da = state->_imu_x_da->id(); + of_state_std << std::sqrt(cov(index_da + 0, index_da + 0)) << " " + << std::sqrt(cov(index_da + 1, index_da + 1)) << " " + << std::sqrt(cov(index_da + 2, index_da + 2)) << " " + << std::sqrt(cov(index_da + 3, index_da + 3)) << " " + << std::sqrt(cov(index_da + 4, index_da + 4)) << " " + << std::sqrt(cov(index_da + 5, index_da + 5)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + + + // tg + of_state_est << state->_imu_x_tg->value()(0) << " " + << state->_imu_x_tg->value()(1) << " " + << state->_imu_x_tg->value()(2) << " " + << state->_imu_x_tg->value()(3) << " " + << state->_imu_x_tg->value()(4) << " " + << state->_imu_x_tg->value()(5) << " " + << state->_imu_x_tg->value()(6) << " " + << state->_imu_x_tg->value()(7) << " " + << state->_imu_x_tg->value()(8) << " "; + if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { + int index_tg = state->_imu_x_tg->id(); + of_state_std << std::sqrt(cov(index_tg + 0, index_tg + 0)) << " " + << std::sqrt(cov(index_tg + 1, index_tg + 1)) << " " + << std::sqrt(cov(index_tg + 2, index_tg + 2)) << " " + << std::sqrt(cov(index_tg + 3, index_tg + 3)) << " " + << std::sqrt(cov(index_tg + 4, index_tg + 4)) << " " + << std::sqrt(cov(index_tg + 5, index_tg + 5)) << " " + << std::sqrt(cov(index_tg + 6, index_tg + 6)) << " " + << std::sqrt(cov(index_tg + 7, index_tg + 7)) << " " + << std::sqrt(cov(index_tg + 8, index_tg + 8)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + + // R_gyrotoI + of_state_est << state->_imu_quat_gyrotoI->value()(0) << " " + << state->_imu_quat_gyrotoI->value()(1) << " " + << state->_imu_quat_gyrotoI->value()(2) << " " + << state->_imu_quat_gyrotoI->value()(3) << " "; + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { + int index_wtoI = state->_imu_quat_gyrotoI->id(); + of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " + << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " " + << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " + << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + + + // R_acctoI + of_state_est << state->_imu_quat_acctoI->value()(0) << " " + << state->_imu_quat_acctoI->value()(1) << " " + << state->_imu_quat_acctoI->value()(2) << " " + << state->_imu_quat_acctoI->value()(3) << " "; + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { + int index_atoI = state->_imu_quat_acctoI->id(); + of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " + << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " " + << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " + << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + // Done with the estimates! of_state_est << endl; of_state_std << endl; diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index aab947cbc..3fa743c49 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -299,20 +299,29 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto } // Transform omega and linear acceleration into imu frame - Eigen::Vector3d omega_inI = w_IinI; Eigen::Vector3d gravity; gravity << 0.0, 0.0, params.gravity_mag; Eigen::Vector3d accel_inI = R_GtoI * (a_IinG + gravity); + Eigen::Vector3d omega_inI = w_IinI; + + // get the readings with the imu intrinsics + Eigen::Matrix3d Tg = params.imu_config.Tg(); + Eigen::Matrix3d Tw = params.imu_config.Tw(); + Eigen::Matrix3d Ta = params.imu_config.Ta(); + Eigen::Vector3d omega_inw = Tw * params.imu_config.R_ItoGyro() * omega_inI + Tg * accel_inI; + Eigen::Vector3d accel_ina = Ta * params.imu_config.R_ItoAcc() * R_GtoI * accel_inI; + + // Now add noise to these measurements double dt = 1.0 / params.sim_freq_imu; std::normal_distribution w(0, 1); - wm(0) = omega_inI(0) + true_bias_gyro(0) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(1) = omega_inI(1) + true_bias_gyro(1) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(2) = omega_inI(2) + true_bias_gyro(2) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - am(0) = accel_inI(0) + true_bias_accel(0) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(1) = accel_inI(1) + true_bias_accel(1) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(2) = accel_inI(2) + true_bias_accel(2) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + wm(0) = omega_inw(0) + true_bias_gyro(0) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(1) = omega_inw(1) + true_bias_gyro(1) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(2) = omega_inw(2) + true_bias_gyro(2) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + am(0) = accel_ina(0) + true_bias_accel(0) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(1) = accel_ina(1) + true_bias_accel(1) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(2) = accel_ina(2) + true_bias_accel(2) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); // Move the biases forward in time true_bias_gyro(0) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index dcf3210d5..96adcaaaf 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -64,8 +64,8 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest // Q_summed = Phi_i*Q_summed*Phi_i^T + Q_i // After summing we can multiple the total phi to get the updated covariance // We will then add the noise to the IMU portion of the state - Eigen::Matrix Phi_summed = Eigen::Matrix::Identity(); - Eigen::Matrix Qd_summed = Eigen::Matrix::Zero(); + Eigen::MatrixXd Phi_summed = Eigen::MatrixXd::Identity(state->imu_intrinsic_size()+15,state->imu_intrinsic_size()+15); + Eigen::MatrixXd Qd_summed = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15,state->imu_intrinsic_size()+15); double dt_summed = 0; // Loop through all IMU messages, and use them to move the state forward in time @@ -74,8 +74,7 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest for (size_t i = 0; i < prop_data.size() - 1; i++) { // Get the next state Jacobian and noise Jacobian for this IMU reading - Eigen::Matrix F = Eigen::Matrix::Zero(); - Eigen::Matrix Qdi = Eigen::Matrix::Zero(); + Eigen::MatrixXd F, Qdi; predict_and_compute(state, prop_data.at(i), prop_data.at(i + 1), F, Qdi); // Next we should propagate our IMU covariance @@ -91,16 +90,37 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest } } +// // Last angular velocity (used for cloning when estimating time offset) +// Eigen::Matrix last_w = Eigen::Matrix::Zero(); +// if (prop_data.size() > 1) +// last_w = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); +// else if (!prop_data.empty()) +// last_w = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); + // Last angular velocity (used for cloning when estimating time offset) - Eigen::Matrix last_w = Eigen::Matrix::Zero(); - if (prop_data.size() > 1) - last_w = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); - else if (!prop_data.empty()) - last_w = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); + // Remember to correct them before we store them + Eigen::Matrix last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size()-1).am - state->_imu->bias_a()); + Eigen::Matrix last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; + Eigen::Matrix last_w = state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size()-1).wm - state->_imu->bias_g() - state->Tg()*last_a); + + // Do the update to the covariance with our "summed" state transition and IMU noise addition... std::vector> Phi_order; Phi_order.push_back(state->_imu); + if(state->_options.do_calib_imu_intrinsics){ + Phi_order.push_back(state->_imu_x_dw); + Phi_order.push_back(state->_imu_x_da); + if(state->_options.do_calib_imu_g_sensitivity){ + Phi_order.push_back(state->_imu_x_tg); + } + if(state->_options.imu_model == 0){ + Phi_order.push_back(state->_imu_quat_gyrotoI); + }else{ + Phi_order.push_back(state->_imu_quat_acctoI); + } + } + StateHelper::EKFPropagation(state, Phi_order, Phi_order, Phi_summed, Qd_summed); // Set timestamp data @@ -297,94 +317,76 @@ std::vector Propagator::select_imu_readings(const std::vector< } void Propagator::predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, - Eigen::Matrix &F, Eigen::Matrix &Qd) { + Eigen::MatrixXd &F, Eigen::MatrixXd &Qd) { // Set them to zero F.setZero(); - Qd.setZero(); // Time elapsed over interval double dt = data_plus.timestamp - data_minus.timestamp; // assert(data_plus.timestamp>data_minus.timestamp); - // Corrected imu measurements - Eigen::Matrix w_hat = data_minus.wm - state->_imu->bias_g(); - Eigen::Matrix a_hat = data_minus.am - state->_imu->bias_a(); - Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g(); + // Corrected imu acc measurements with our current biases + Eigen::Matrix a_hat1 = data_minus.am - state->_imu->bias_a(); Eigen::Matrix a_hat2 = data_plus.am - state->_imu->bias_a(); + Eigen::Matrix a_hat = a_hat1; + if (state->_options.imu_avg) { + a_hat = .5*(a_hat1+a_hat2); + } + // Imu acc readings in raw frame + Eigen::Vector3d a_uncorrected = a_hat; + // Correct imu readings with IMU intrinsics + a_hat = state->R_AcctoI() * state->Da() * a_hat; + a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; + a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; + + + // gravity sensitivity correction + Eigen::Matrix gravity_correction1 = state->Tg() * a_hat1; + Eigen::Matrix gravity_correction2 = state->Tg() * a_hat2; + + // Corrected imu gyro measurements with our current biases + Eigen::Matrix w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; + Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; + Eigen::Matrix w_hat = w_hat1; + if (state->_options.imu_avg) { + w_hat = .5*(w_hat1+w_hat2); + } + // Imu readings in raw frame + Eigen::Vector3d w_uncorrected = w_hat; + + // Correct imu readings with IMU intrinsics + w_hat = state->R_GyrotoI() * state->Dw() * w_hat; + w_hat1 = state->R_GyrotoI() * state->Dw() * w_hat1; + w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; + + // Pre-compute some analytical values + Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3,18); + if (state->_options.use_analytic_integration) { + compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); + } // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if (state->_options.use_rk4_integration) + if(state->_options.use_analytic_integration) + predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); + else if (state->_options.use_rk4_integration) predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); else predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); - // Get the locations of each entry of the imu state - int th_id = state->_imu->q()->id() - state->_imu->id(); - int p_id = state->_imu->p()->id() - state->_imu->id(); - int v_id = state->_imu->v()->id() - state->_imu->id(); - int bg_id = state->_imu->bg()->id() - state->_imu->id(); - int ba_id = state->_imu->ba()->id() - state->_imu->id(); - - // Allocate noise Jacobian - Eigen::Matrix G = Eigen::Matrix::Zero(); - - // Now compute Jacobian of new state wrt old state and noise - if (state->_options.do_fej) { - - // This is the change in the orientation from the end of the last prop to the current prop - // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix Rfej = state->_imu->Rot_fej(); - Eigen::Matrix dR = quat_2_Rot(new_q) * Rfej.transpose(); - - Eigen::Matrix v_fej = state->_imu->vel_fej(); - Eigen::Matrix p_fej = state->_imu->pos_fej(); - - F.block(th_id, th_id, 3, 3) = dR; - F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-w_hat * dt) * dt; - // F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-log_so3(dR)) * dt; - F.block(bg_id, bg_id, 3, 3).setIdentity(); - F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v - v_fej + _gravity * dt) * Rfej.transpose(); - // F.block(v_id, th_id, 3, 3).noalias() = -Rfej.transpose() * skew_x(Rfej*(new_v-v_fej+_gravity*dt)); - F.block(v_id, v_id, 3, 3).setIdentity(); - F.block(v_id, ba_id, 3, 3) = -Rfej.transpose() * dt; - F.block(ba_id, ba_id, 3, 3).setIdentity(); - F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p - p_fej - v_fej * dt + 0.5 * _gravity * dt * dt) * Rfej.transpose(); - // F.block(p_id, th_id, 3, 3).noalias() = -0.5 * Rfej.transpose() * skew_x(2*Rfej*(new_p-p_fej-v_fej*dt+0.5*_gravity*dt*dt)); - F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; - F.block(p_id, ba_id, 3, 3) = -0.5 * Rfej.transpose() * dt * dt; - F.block(p_id, p_id, 3, 3).setIdentity(); - - G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-w_hat * dt) * dt; - // G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-log_so3(dR)) * dt; - G.block(v_id, 3, 3, 3) = -Rfej.transpose() * dt; - G.block(p_id, 3, 3, 3) = -0.5 * Rfej.transpose() * dt * dt; - G.block(bg_id, 6, 3, 3) = Eigen::Matrix::Identity(); - G.block(ba_id, 9, 3, 3) = Eigen::Matrix::Identity(); - } else { - - Eigen::Matrix R_Gtoi = state->_imu->Rot(); + // Allocate state transition and noise Jacobian + F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15,12); - F.block(th_id, th_id, 3, 3) = exp_so3(-w_hat * dt); - F.block(th_id, bg_id, 3, 3).noalias() = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; - F.block(bg_id, bg_id, 3, 3).setIdentity(); - F.block(v_id, th_id, 3, 3).noalias() = -R_Gtoi.transpose() * skew_x(a_hat * dt); - F.block(v_id, v_id, 3, 3).setIdentity(); - F.block(v_id, ba_id, 3, 3) = -R_Gtoi.transpose() * dt; - F.block(ba_id, ba_id, 3, 3).setIdentity(); - F.block(p_id, th_id, 3, 3).noalias() = -0.5 * R_Gtoi.transpose() * skew_x(a_hat * dt * dt); - F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; - F.block(p_id, ba_id, 3, 3) = -0.5 * R_Gtoi.transpose() * dt * dt; - F.block(p_id, p_id, 3, 3).setIdentity(); - - G.block(th_id, 0, 3, 3) = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; - G.block(v_id, 3, 3, 3) = -R_Gtoi.transpose() * dt; - G.block(p_id, 3, 3, 3) = -0.5 * R_Gtoi.transpose() * dt * dt; - G.block(bg_id, 6, 3, 3) = Eigen::Matrix::Identity(); - G.block(ba_id, 9, 3, 3) = Eigen::Matrix::Identity(); + if(state->_options.use_analytic_integration){ + compute_F_and_G_analytic(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, + new_q, new_v, new_p, Xi_sum, F, G); + } else { + compute_F_and_G_discrete(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, + new_q, new_v, new_p, F, G); } // Construct our discrete noise covariance matrix @@ -397,6 +399,7 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core Qc.block(9, 9, 3, 3) = _imu_config.sigma_ab_2 * dt * Eigen::Matrix::Identity(); // Compute the noise injected into the state over the interval + Qd = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); Qd = G * Qc * G.transpose(); Qd = 0.5 * (Qd + Qd.transpose()); @@ -409,6 +412,309 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core state->_imu->set_fej(imu_x); } +void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, + const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, + const Eigen::Matrix &Xi_sum, + Eigen::MatrixXd &F, Eigen::MatrixXd &G) { + + //============================================================ + //============================================================ + + // Get the locations of each entry of the imu state + int local_size = 0; + int th_id = local_size; local_size += state->_imu->q()->size(); + int p_id = local_size; local_size += state->_imu->p()->size(); + int v_id = local_size; local_size += state->_imu->v()->size(); + int bg_id = local_size; local_size += state->_imu->bg()->size(); + int ba_id = local_size; local_size += state->_imu->ba()->size(); + + // If we are doing calibration, we can define their "local" id in the state transition + int Dw_id = -1; + int Da_id = -1; + int Tg_id = -1; + int th_atoI_id = -1; + int th_wtoI_id = -1; + if(state->_options.do_calib_imu_intrinsics) { + Dw_id = local_size; + local_size += state->_imu_x_dw->size(); + Da_id = local_size; + local_size += state->_imu_x_da->size(); + if(state->_options.do_calib_imu_g_sensitivity){ + Tg_id = local_size; + local_size += state->_imu_x_tg->size(); + } + if(state->_options.imu_model == 0){ + // Kalibr model + th_wtoI_id = local_size; + local_size += state->_imu_quat_gyrotoI->size(); + }else{ + // RPNG model + th_atoI_id = local_size; + local_size += state->_imu_quat_acctoI->size(); + } + } + + + //============================================================ + //============================================================ + + // This is the change in the orientation from the end of the last prop to the current prop + // This is needed since we need to include the "k-th" updated orientation information + Eigen::Matrix R_k = state->_imu->Rot(); + Eigen::Matrix v_k = state->_imu->vel(); + Eigen::Matrix p_k = state->_imu->pos(); + + if(state->_options.do_fej){ + R_k = state->_imu->Rot_fej(); + v_k = state->_imu->vel_fej(); + p_k = state->_imu->pos_fej(); + } + Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q)*R_k.transpose(); + + + Eigen::Matrix Da = state->Da(); + Eigen::Matrix Dw = state->Dw(); + Eigen::Matrix Tg = state->Tg(); + Eigen::Matrix R_atoI = state->R_AcctoI(); + Eigen::Matrix R_wtoI = state->R_GyrotoI(); + Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; + Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected;// contains the gravity correction already + + Eigen::Matrix3d Xi_1 = Xi_sum.block<3,3>(0,3); + Eigen::Matrix3d Xi_2 = Xi_sum.block<3,3>(0,6); + Eigen::Matrix3d Jr = Xi_sum.block<3,3>(0,9); + Eigen::Matrix3d Xi_3 = Xi_sum.block<3,3>(0,12); + Eigen::Matrix3d Xi_4 = Xi_sum.block<3,3>(0,15); + + // for th + F.block(th_id, th_id, 3, 3) = dR_ktok1; + F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); + F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); + + // for p + F.block(p_id, p_id, 3, 3).setIdentity(); + + // for v + F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; + F.block(v_id, v_id, 3, 3).setIdentity(); + + // for bg + F.block(th_id, bg_id, 3, 3).noalias() = -Jr * dt * R_wtoI * Dw; + F.block(p_id, bg_id, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI* Dw; + F.block(v_id, bg_id, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI* Dw; + F.block(bg_id, bg_id, 3, 3).setIdentity(); + + // for ba + F.block(th_id, ba_id, 3, 3).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block(p_id, ba_id, 3, 3) = -R_k.transpose()*(Xi_2+Xi_4*R_wtoI*Dw*Tg)*R_atoI*Da; + F.block(v_id, ba_id, 3, 3) = -R_k.transpose()*(Xi_1+Xi_3*R_wtoI*Dw*Tg)*R_atoI*Da; + F.block(ba_id, ba_id, 3, 3).setIdentity(); + + + // begin to add the state transition matrix for the omega intrinsics part + if(Dw_id != -1){ + // F.block(Dw_id, Dw_id, state->get_imu_intrinsics_gyro()->size(), state->get_imu_intrinsics_gyro()->size()).setIdentity(); + F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); + F.block(p_id, Dw_id, 3, state->_imu_x_dw->size()) = - R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state,w_uncorrected); + F.block(v_id, Dw_id, 3, state->_imu_x_dw->size()) = - R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state,w_uncorrected); + F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); + } + + // begin to add the state transition matrix for the acc intrinsics part + if(Da_id != -1){ + F.block(Da_id, Da_id, state->_imu_x_da->size(), state->_imu_x_da->size()).setIdentity(); + F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * compute_H_Da(state,w_uncorrected); + F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * ( Xi_2 + Xi_4 * R_wtoI * Dw * Tg ) * R_atoI * compute_H_Da(state,a_uncorrected); + F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg )* R_atoI * compute_H_Da(state,a_uncorrected); + } + + if(Tg_id != -1){ + F.block(Tg_id,Tg_id,state->_imu_x_tg->size(),state->_imu_x_tg->size()).setIdentity(); + F.block(th_id,Tg_id,3,state->_imu_x_tg->size()) = -Jr*dt*R_wtoI*Dw*compute_H_Tg(state,a_k); + F.block(p_id,Tg_id,3,state->_imu_x_tg->size())=R_k.transpose()*Xi_4*R_wtoI*Dw*compute_H_Tg(state,a_k); + F.block(v_id,Tg_id,3,state->_imu_x_tg->size())=R_k.transpose()*Xi_3*R_wtoI*Dw*compute_H_Tg(state,a_k); + } + + // begin to add the state transition matrix for the acc intrinsics part + if(th_atoI_id != -1){ + F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); + F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block(p_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); + F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); + } + + + if(th_wtoI_id != -1){ + F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); + F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); + F.block(p_id, th_wtoI_id, 3, 3) = - R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); + F.block(v_id, th_wtoI_id, 3, 3) = - R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); + } + + + G.block(th_id, 0, 3, 3) = -Jr * dt *R_wtoI * Dw; + G.block(p_id, 0, 3, 3) = R_k.transpose() * Xi_4 *R_wtoI * Dw; + G.block(v_id, 0, 3, 3) = R_k.transpose() * Xi_3 *R_wtoI * Dw; + //G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-log_so3(dR)) * dt; + G.block(th_id,3,3,3) = Jr*dt*R_wtoI*Dw*Tg*R_atoI*Da; + G.block(p_id, 3, 3, 3) = -R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) * R_atoI * Da; + G.block(v_id, 3, 3, 3) = -R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) * R_atoI * Da; + G.block(bg_id, 6, 3, 3) = dt*Eigen::Matrix::Identity(); + G.block(ba_id, 9, 3, 3) = dt*Eigen::Matrix::Identity(); + +} + + +void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, + const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, + Eigen::MatrixXd &F, Eigen::MatrixXd &G) { + + //============================================================ + //============================================================ + + // Get the locations of each entry of the imu state + int local_size = 0; + int th_id = local_size; local_size += state->_imu->q()->size(); + int p_id = local_size; local_size += state->_imu->p()->size(); + int v_id = local_size; local_size += state->_imu->v()->size(); + int bg_id = local_size; local_size += state->_imu->bg()->size(); + int ba_id = local_size; local_size += state->_imu->ba()->size(); + + // If we are doing calibration, we can define their "local" id in the state transition + int Dw_id = -1; + int Da_id = -1; + int Tg_id = -1; + int th_atoI_id = -1; + int th_wtoI_id = -1; + if(state->_options.do_calib_imu_intrinsics) { + Dw_id = local_size; + local_size += state->_imu_x_dw->size(); + Da_id = local_size; + local_size += state->_imu_x_da->size(); + if(state->_options.do_calib_imu_g_sensitivity){ + Tg_id = local_size; + local_size += state->_imu_x_tg->size(); + } + if(state->_options.imu_model == 0){ + // Kalibr model + th_wtoI_id = local_size; + local_size += state->_imu_quat_gyrotoI->size(); + }else{ + // RPNG model + th_atoI_id = local_size; + local_size += state->_imu_quat_acctoI->size(); + } + } + + + //============================================================ + //============================================================ + + // This is the change in the orientation from the end of the last prop to the current prop + // This is needed since we need to include the "k-th" updated orientation information + Eigen::Matrix R_k = state->_imu->Rot(); + Eigen::Matrix v_k = state->_imu->vel(); + Eigen::Matrix p_k = state->_imu->pos(); + + if(state->_options.do_fej){ + R_k = state->_imu->Rot_fej(); + v_k = state->_imu->vel_fej(); + p_k = state->_imu->pos_fej(); + } + Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q)*R_k.transpose(); + + + // This is the change in the orientation from the end of the last prop to the current prop + // This is needed since we need to include the "k-th" updated orientation information + + + Eigen::Matrix Da = state->Da(); + Eigen::Matrix Dw = state->Dw(); + Eigen::Matrix Tg = state->Tg(); + Eigen::Matrix R_atoI = state->R_AcctoI(); + Eigen::Matrix R_wtoI = state->R_GyrotoI(); + Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; + Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected;// contains the gravity correction already + Eigen::Matrix Jr = Jr_so3(w_k * dt); + + // for theta + F.block(th_id, th_id, 3, 3) = dR_ktok1; + //F.block(th_id, bg_id, 3, 3).noalias() = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; + F.block(th_id, bg_id, 3, 3).noalias() = -dR_ktok1 * Jr_so3(-w_hat * dt) * dt * R_wtoI * Dw; + F.block(th_id, ba_id, 3, 3).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + + // for position + F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); + F.block(p_id, p_id, 3, 3).setIdentity(); + F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; + F.block(p_id, ba_id, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + + // for velocity + F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); + F.block(v_id, v_id, 3, 3).setIdentity(); + F.block(v_id, ba_id, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; + + // for bg + F.block(bg_id, bg_id, 3, 3).setIdentity(); + // for ba + F.block(ba_id, ba_id, 3, 3).setIdentity(); + + + + // begin to add the state transition matrix for the omega intrinsics part + if(Dw_id != -1) { + F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); + F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr_so3(w_hat * dt) * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); + } + + // begin to add the state transition matrix for the acc intrinsics part + if(Da_id != -1) { + F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI*Tg*R_atoI*compute_H_Da(state,a_uncorrected); + F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * compute_H_Da(state,a_uncorrected); + F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * dt * R_atoI * compute_H_Da(state,a_uncorrected); + F.block(Da_id, Da_id, state->_imu_x_da->size(), state->_imu_x_da->size()).setIdentity(); + } + + // begin to add the state transition matrix for the acc intrinsics part + if(th_atoI_id != -1) { + F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); + F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block(p_id, th_atoI_id, 3, 3) = 0.5 * R_k.transpose() * dt * dt *ov_core::skew_x(a_k); + F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * dt *ov_core::skew_x(a_k); + } + + // begin to add the state transition matrix for the gyro intrinsics part + if(th_wtoI_id != -1) { + F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); + F.block(th_id, th_wtoI_id, 3, 3) = Jr_so3(w_hat * dt) * dt * ov_core::skew_x(w_k); + } + + // begin to add the state transition matrix for the gravity sensitivity part + if(Tg_id != -1){ + F.block(Tg_id, Tg_id, state->_imu_x_tg->size(),state->_imu_x_tg->size()).setIdentity(); + F.block(th_id, Tg_id, 3, state->_imu_x_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state,a_k); + } + + // Noise jacobian + G.block(th_id, 0, 3, 3) = -Jr_so3(-w_hat * dt) * dt * R_wtoI * Dw; + G.block(th_id, 3, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block(v_id, 3, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; + G.block(p_id, 3, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + G.block(bg_id, 6, 3, 3) = dt*Eigen::Matrix::Identity(); + G.block(ba_id, 9, 3, 3) = dt*Eigen::Matrix::Identity(); + +} + + + + + + + void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { @@ -443,6 +749,35 @@ void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, new_p = state->_imu->pos() + state->_imu->vel() * dt + 0.5 * R_Gtoi.transpose() * a_hat * dt * dt - 0.5 * _gravity * dt * dt; } + +void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, + Eigen::Matrix& Xi_sum) { + + // Pre-compute things + Eigen::Matrix R_Gtok = state->_imu->Rot(); + + // get the pre-computed value + Eigen::Matrix3d R_k1_to_k = Xi_sum.block(0,0,3,3); + Eigen::Matrix3d Xi_1 = Xi_sum.block<3,3>(0,3); + Eigen::Matrix3d Xi_2 = Xi_sum.block<3,3>(0,6); + + // use the precomputed value to get the new state + Eigen::Matrix q_k_to_k1 = ov_core::rot_2_quat(R_k1_to_k.transpose()); + new_q = ov_core::quat_multiply(q_k_to_k1, state->_imu->quat()); + + // Velocity: just the acceleration in the local frame, minus global gravity + new_v = state->_imu->vel() + R_Gtok.transpose()* Xi_1 *a_hat - _gravity*dt; + + // Position: just velocity times dt, with the acceleration integrated twice + new_p = state->_imu->pos() + state->_imu->vel()*dt + R_Gtok.transpose()*Xi_2*a_hat - 0.5*_gravity*dt*dt; + + +} + + + void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { @@ -523,3 +858,184 @@ void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const new_p = p_0 + (1.0 / 6.0) * k1_p + (1.0 / 3.0) * k2_p + (1.0 / 3.0) * k3_p + (1.0 / 6.0) * k4_p; new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v; } + + +void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Matrix &Xi_sum) { + + // useful identities + Eigen::Matrix3d I_3x3 = Eigen::Matrix3d::Identity(); + Eigen::Vector3d Z_3x1 = Eigen::MatrixXd::Zero(3,1); + + // now begin the integration + double w_norm = w_hat.norm(); + double d_th = w_norm * d_t; + Eigen::Vector3d k_hat; + if(w_norm < 1e-8) { k_hat << Z_3x1;} + else { k_hat << w_hat / w_norm;} + + // comupute usefull identities + double d_t2 = std::pow(d_t,2); + double d_t3 = std::pow(d_t,3);; + double w_norm2 = std::pow(w_norm,2); + double w_norm3 = std::pow(w_norm,3); + double cos_dth = cos(d_th); + double sin_dth = sin(d_th); + double d_th2 = std::pow(d_th,2); + double d_th3 = std::pow(d_th,3); + Eigen::Matrix3d sK = ov_core::skew_x(k_hat); + Eigen::Matrix3d sK2 = sK * sK; + Eigen::Matrix3d sA = ov_core::skew_x(a_hat); + + // based on the delta theta, let's decide which integration will be used + bool small_w = (w_norm < 1.0/180 * M_PI / 2); + bool small_th = (d_th < 1.0/180 * M_PI / 2); + + // integration components will be used later + Eigen::Matrix3d R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; + + // now the R and J can be computed + R_k1tok = I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2; + if(!small_th){ + Jr_k1tok = I_3x3 - (1.0-cos_dth) / d_th * sK + (d_th - sin_dth) / d_th * sK2; + }else{ + Jr_k1tok = I_3x3 - sin_dth * sK + (1.0 - cos_dth) * sK2; + } + + // now begin the integration + if(!small_w){ + + // first order rotation integration with constant omega + Xi_1 = I_3x3 * d_t + + (1.0 - cos_dth) / w_norm * sK + + (d_t - sin_dth / w_norm) * sK2; + + // second order rotation integration with constat omega + Xi_2 = 1.0/2 * d_t2 * I_3x3 + + (d_th - sin_dth) / w_norm2 * sK + + (1.0/2 * d_t2 - (1.0-cos_dth) / w_norm2) * sK2; + + // first order RAJ integratioin with constant omega and constant acc + Xi_3 = 1.0/2 * d_t2 * sA + + (sin_dth - d_th) / w_norm2 * sA * sK + + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA + + (1.0/2*d_t2 - (1.0-cos_dth) / w_norm2) * sA * sK2 + + (1.0/2*d_t2 - (1.0-cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) + - (3*sin_dth - 2*d_th- d_th*cos_dth) / w_norm2 * k_hat.dot(a_hat)*sK2; + + // second order RAJ integration with constant omega and constant acc + Xi_4 = 1.0/6 * d_t3 * sA + + (2*(1.0-cos_dth) - d_th2) / (2*w_norm3) * sA * sK + + ((2*(1.0-cos_dth) - d_th * sin_dth)/w_norm3) * sK * sA + + ((sin_dth - d_th)/w_norm3 + d_t3/6) * sA *sK2 + + ((d_th - 2*sin_dth + 1.0/6*d_th3+d_th*cos_dth)/w_norm3) * (sK2 * sA + k_hat.dot(a_hat) * sK) + + (4*cos_dth -4 + d_th2 + d_th*sin_dth)/w_norm3 * k_hat.dot(a_hat) * sK2; + + }else { + + // first order rotation + Xi_1 = d_t * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); + + // second order rotation + Xi_2 = 1.0 / 2 * d_t * Xi_1; + //iint_R = 1.0/2 * d_t2 * (I_3x3 + sin_dth * sK + (1.0-cos_dth) * sK2); + + // first order RAJ + Xi_3 = 1.0 / 2 * d_t2 * (sA + + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) + + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); + + // second order RAJ + Xi_4 = 1.0 / 3 * d_t * Xi_3; + + } + + // store the integrated parameters + Xi_sum << R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; + + + // we are good to go + return; +} + + + +Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected) { + + Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3,3); + Eigen::Vector3d e_1 = I_3x3.block<3,1>(0,0); + Eigen::Vector3d e_2 = I_3x3.block<3,1>(0,1); + Eigen::Vector3d e_3 = I_3x3.block<3,1>(0,2); + + double w_1 = w_uncorrected(0); + double w_2 = w_uncorrected(1); + double w_3 = w_uncorrected(2); + + // intrinsic parameters + Eigen::MatrixXd H_Dw; + if(state->_options.do_calib_imu_intrinsics) { + H_Dw = Eigen::MatrixXd::Zero(3,6); + if(state->_options.imu_model == 0){ + // Kalibr model + H_Dw << w_1*I_3x3, w_2*e_2, w_2*e_3, w_3*e_3; + }else{ + // RPNG model + H_Dw << w_1*e_1, w_2*e_1, w_2*e_2, w_3*I_3x3; + } + } + + // we are good to go + return H_Dw; +} + +Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected) { + + Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3,3); + Eigen::Vector3d e_1 = I_3x3.block<3,1>(0,0); + Eigen::Vector3d e_2 = I_3x3.block<3,1>(0,1); + Eigen::Vector3d e_3 = I_3x3.block<3,1>(0,2); + + double a_1 = a_uncorrected(0); + double a_2 = a_uncorrected(1); + double a_3 = a_uncorrected(2); + + // intrinsic parameters + Eigen::MatrixXd H_Da; + if(state->_options.do_calib_imu_intrinsics) { + H_Da = Eigen::MatrixXd::Zero(3,6); + if(state->_options.imu_model == 0){ + // kalibr model + H_Da << a_1*I_3x3, a_2*e_2, a_2*e_3, a_3*e_3; + }else{ + // RPNG model + H_Da << a_1*e_1, a_2*e_1, a_2*e_2, a_3*I_3x3; + } + + } + + + // we are good to go + return H_Da; +} + + +Eigen::MatrixXd Propagator::compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI) { + + Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3,3); + Eigen::Vector3d e_1 = I_3x3.block<3,1>(0,0); + Eigen::Vector3d e_2 = I_3x3.block<3,1>(0,1); + double a_1 = a_inI(0); + double a_2 = a_inI(1); + double a_3 = a_inI(2); + + // intrinsic parameters + Eigen::MatrixXd H_Tg; + + if(state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity){ + H_Tg = Eigen::MatrixXd::Zero(3,9); + H_Tg << a_1*I_3x3, a_2*I_3x3, a_3*I_3x3; + } + + // we are good to go + return H_Tg; +} \ No newline at end of file diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 6df21e920..1c291a5ac 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -162,7 +162,7 @@ class Propagator { * @param Qd Discrete-time noise covariance over the interval */ void predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, - Eigen::Matrix &F, Eigen::Matrix &Qd); + Eigen::MatrixXd &F, Eigen::MatrixXd &Qd); /** * @brief Discrete imu mean propagation. @@ -224,6 +224,49 @@ class Propagator { const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p); + + /// TODO: comment this.... + void predict_mean_analytic(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, + Eigen::Matrix &Xi_sum); + + + + /// TODO: comment this.... + void compute_Xi_sum(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Matrix &Xi_sum); + + /// TODO: comment this.... + void compute_F_and_G_analytic(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, + const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, + const Eigen::Matrix &Xi_sum, + Eigen::MatrixXd &F, Eigen::MatrixXd &G); + + /// TODO: comment this.... + void compute_F_and_G_discrete(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, + const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, + Eigen::MatrixXd &F, Eigen::MatrixXd &G); + + /// TODO: comment this.... + Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); + + /// TODO: comment this.... + Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); + + /// TODO: comment this.... + Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); + + + + + + /// Container for the noise values ov_core::ImuConfig _imu_config; diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index f7163865b..e75cea930 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -37,6 +37,44 @@ State::State(StateOptions &options) { _variables.push_back(_imu); current_id += _imu->size(); + // Append the imu intrinsics to the state and covariance + _imu_x_dw = std::make_shared(6); + _imu_x_da = std::make_shared(6); + _imu_x_tg = std::make_shared(9); + _imu_quat_gyrotoI = std::make_shared(); + _imu_quat_acctoI = std::make_shared(); + + + if(options.do_calib_imu_intrinsics) { + _imu_x_dw->set_local_id(current_id); + _variables.push_back(_imu_x_dw); + current_id += _imu_x_dw->size(); + + _imu_x_da->set_local_id(current_id); + _variables.push_back(_imu_x_da); + current_id += _imu_x_da->size(); + + if(options.do_calib_imu_g_sensitivity){ + _imu_x_tg->set_local_id(current_id); + _variables.push_back(_imu_x_tg); + current_id += _imu_x_tg->size(); + } + + if(options.imu_model == 0){ + // kalibr model + _imu_quat_gyrotoI->set_local_id(current_id); + _variables.push_back(_imu_quat_gyrotoI); + current_id += _imu_quat_gyrotoI->size(); + }else{ + // rpng model + _imu_quat_acctoI->set_local_id(current_id); + _variables.push_back(_imu_quat_acctoI); + current_id += _imu_quat_acctoI->size(); + } + } + + + // Camera to IMU time offset _calib_dt_CAMtoIMU = std::make_shared(1); if (_options.do_calib_camera_timeoffset) { @@ -77,6 +115,20 @@ State::State(StateOptions &options) { _Cov = 1e-3 * Eigen::MatrixXd::Identity(current_id, current_id); // Finally, set some of our priors for our calibration parameters + if(_options.do_calib_imu_intrinsics){ + _Cov.block<6,6>(_imu_x_dw->id(),_imu_x_dw->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); + _Cov.block<6,6>(_imu_x_da->id(),_imu_x_da->id()) = std::pow(0.008,2) * Eigen::Matrix::Identity(); + if(_options.do_calib_imu_g_sensitivity){ + _Cov.block<9,9>(_imu_x_tg->id(),_imu_x_tg->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); + } + if(_options.imu_model == 0){ + _Cov.block<3,3>(_imu_quat_gyrotoI->id(), _imu_quat_gyrotoI->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); + }else{ + _Cov.block<3,3>(_imu_quat_acctoI->id(),_imu_quat_acctoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + } + } + + if (_options.do_calib_camera_timeoffset) { _Cov(_calib_dt_CAMtoIMU->id(), _calib_dt_CAMtoIMU->id()) = std::pow(0.01, 2); } @@ -95,3 +147,52 @@ State::State(StateOptions &options) { } } } + + +Eigen::Matrix3d State::Dw() { + Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); + if(_options.imu_model == 0){ + Dw << _imu_x_dw->value()(0), 0, 0, + _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, + _imu_x_dw->value()(2), _imu_x_dw->value()(4), _imu_x_dw->value()(5); + }else{ + Dw << _imu_x_dw->value()(0), _imu_x_dw->value()(1), _imu_x_dw->value()(3), + 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), + 0, 0, _imu_x_dw->value()(5); + } + return Dw; +} + + +Eigen::Matrix3d State::Da() { + Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); + if(_options.imu_model == 0){ + Da << _imu_x_da->value()(0), 0, 0, + _imu_x_da->value()(1), _imu_x_da->value()(3), 0, + _imu_x_da->value()(2), _imu_x_da->value()(4), _imu_x_da->value()(5); + }else{ + Da << _imu_x_da->value()(0), _imu_x_da->value()(1), _imu_x_da->value()(3), + 0, _imu_x_da->value()(2), _imu_x_da->value()(4), + 0, 0, _imu_x_da->value()(5); + } + return Da; +} + + +Eigen::Matrix3d State::Tg() { + Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); + Tg << _imu_x_tg->value()(0), _imu_x_tg->value()(3), _imu_x_tg->value()(6), + _imu_x_tg->value()(1), _imu_x_tg->value()(4), _imu_x_tg->value()(7), + _imu_x_tg->value()(2), _imu_x_tg->value()(5), _imu_x_tg->value()(8); + + return Tg; +} + + +Eigen::Matrix3d State::R_AcctoI() { + return _imu_quat_acctoI->Rot(); +} + +Eigen::Matrix3d State::R_GyrotoI() { + return _imu_quat_gyrotoI->Rot(); +} diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 99f824996..4dd925fd7 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -78,6 +78,29 @@ class State { */ int max_covariance_size() { return (int)_Cov.rows(); } + + // get Dw + Eigen::Matrix3d Dw(); + // get Da + Eigen::Matrix3d Da(); + // get Tg + Eigen::Matrix3d Tg(); + // get R_AcctoI + Eigen::Matrix3d R_AcctoI(); + // get R_GyrotoI + Eigen::Matrix3d R_GyrotoI(); + + int imu_intrinsic_size() { + int sz = 0; + if(_options.do_calib_imu_intrinsics){ + sz += 15; + if(_options.do_calib_imu_g_sensitivity){ + sz += 9; + } + } + return sz; + } + /// Current timestamp (should be the last update time!) double _timestamp = -1; @@ -105,6 +128,13 @@ class State { /// Camera intrinsics camera objects std::unordered_map> _cam_intrinsics_cameras; + /// IMU intrinsics + std::shared_ptr _imu_x_dw; + std::shared_ptr _imu_x_da; + std::shared_ptr _imu_quat_acctoI; + std::shared_ptr _imu_quat_gyrotoI; + std::shared_ptr _imu_x_tg; + private: // Define that the state helper is a friend class of this class // This will allow it to access the below functions which should normally not be called diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index ebebecd33..3f32a81af 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -255,6 +255,7 @@ class StateHelper { StateHelper() {} }; + } // namespace ov_msckf #endif // OV_MSCKF_STATE_HELPER_H diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index c07ca0bc3..adcf22f99 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -42,7 +42,10 @@ struct StateOptions { bool imu_avg = false; /// Bool to determine if we should use Rk4 imu integration - bool use_rk4_integration = true; + bool use_rk4_integration = false; + + /// Bool to determine if we should use analytic imu integration + bool use_analytic_integration = true; /// Bool to determine whether or not to calibrate imu-to-camera pose bool do_calib_camera_pose = false; @@ -60,7 +63,7 @@ struct StateOptions { bool do_calib_imu_g_sensitivity = false; /// Indicator to use which model, 0: kalibr and 1: rpng - int imu_mode = 0; + int imu_model = 0; /// Max clone size of sliding window int max_clone_size = 11; @@ -95,6 +98,7 @@ struct StateOptions { parser->parse_config("use_fej", do_fej); parser->parse_config("use_imuavg", imu_avg); parser->parse_config("use_rk4int", use_rk4_integration); + parser->parse_config("use_analytic_int", use_analytic_integration); parser->parse_config("calib_cam_extrinsics", do_calib_camera_pose); parser->parse_config("calib_cam_intrinsics", do_calib_camera_intrinsics); parser->parse_config("calib_cam_timeoffset", do_calib_camera_timeoffset); @@ -115,6 +119,16 @@ struct StateOptions { std::string rep3 = ov_type::LandmarkRepresentation::as_string(feat_rep_aruco); parser->parse_config("feat_rep_aruco", rep3); feat_rep_aruco = ov_type::LandmarkRepresentation::from_string(rep3); + + // IMU model + std::string imu_model_str = "kalibr"; + parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); + if(imu_model_str == "kalibr") imu_model = 0; + else if(imu_model_str == "rpng") imu_model = 1; + else { + PRINT_ERROR(RED "StateOption(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); + } + } PRINT_DEBUG(" - use_fej: %d\n", do_fej); PRINT_DEBUG(" - use_imuavg: %d\n", imu_avg); From d7448a63ce7f0776aaf62c5ba800ac0dcf684199 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 08:50:53 -0800 Subject: [PATCH 05/60] add the evaluation part for the IMU intrinsics --- ov_eval/src/calc/ResultSimulation.cpp | 255 ++++++++++++++++++++++++++ ov_eval/src/calc/ResultSimulation.h | 2 + ov_eval/src/error_simulation.cpp | 55 +++--- 3 files changed, 287 insertions(+), 25 deletions(-) diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 2d0c96234..b3282354f 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -498,3 +498,258 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { #endif } + + +void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { + + // Check that we have cameras + if ((int)est_state.at(0)(18) < 1) { + PRINT_ERROR(YELLOW "You need at least one camera to run estimator and plot results...\n" RESET); + return; + } + + int num_cam = 1 + 16 + 1; + int imu_model = (int) est_state.at(0)(1 + 16 + 1 + 1 + 15 * num_cam); + int dw_id = 1 + 16 + 1 + 1 + 15 * num_cam + 1; + int dw_cov_id = 1 + 15 + 1 + 1 + 14 * num_cam + 1; + int da_id = dw_id + 6; + int da_cov_id = dw_cov_id + 6; + int tg_id = da_id + 6; + int tg_cov_id = da_cov_id + 6; + int wtoI_id = tg_id + 9; + int wtoI_cov_id = tg_cov_id + 9; + int atoI_id = wtoI_id + 4; + int atoI_cov_id = wtoI_cov_id + 3; + + // Camera extrinsics statistic storage + std::vector error_dw, error_da, error_tg, error_wtoI, error_atoI; + + for (int j = 0; j < 6; j++) { + error_dw.emplace_back(Statistics()); + error_da.emplace_back(Statistics()); + } + for (int j = 0; j < 9; j++) { + error_tg.emplace_back(Statistics()); + } + for (int j = 0; j < 3; j++) { + error_wtoI.emplace_back(Statistics()); + error_atoI.emplace_back(Statistics()); + } + + + // Loop through and calculate error + double start_time = est_state.at(0)(0); + for (size_t i = 0; i < est_state.size(); i++) { + + // Exit if we have reached our max time + if ((est_state.at(i)(0) - start_time) > max_time) + break; + + // Assert our times are the same + assert(est_state.at(i)(0) == gt_state.at(i)(0)); + + // If we are not calibrating then don't plot it! + if (state_cov.at(i)(dw_cov_id) == 0.0) { + PRINT_WARNING(YELLOW "IMU intrinsics not calibrated online, so will not plot...\n" RESET); + return; + } + + // Loop through IMU parameters and calculate error + for(int j=0; j<6; j++){ + error_dw.at(j).timestamps.push_back(est_state.at(i)(0)); + error_dw.at(j).values.push_back(est_state.at(i)(dw_id + j) - gt_state.at(i)(dw_id + j)); + error_dw.at(j).values_bound.push_back(3 * state_cov.at(i)(dw_cov_id + j)); + error_da.at(j).timestamps.push_back(est_state.at(i)(0)); + error_da.at(j).values.push_back(est_state.at(i)(da_id + j) - gt_state.at(i)(da_id + j)); + error_da.at(j).values_bound.push_back(3 * state_cov.at(i)(da_cov_id + j)); + } + + for(int j=0; j<9; j++){ + error_tg.at(j).timestamps.push_back(est_state.at(i)(0)); + error_tg.at(j).values.push_back(est_state.at(i)(tg_id + j) - gt_state.at(i)(tg_id + j)); + error_tg.at(j).values_bound.push_back(3 * state_cov.at(i)(tg_cov_id + j)); + + } + + + Eigen::Matrix3d e_R_wtoI = ov_core::quat_2_Rot(gt_state.at(i).block<4,1>(wtoI_id, 0)) * + ov_core::quat_2_Rot(est_state.at(i).block<4,1>(wtoI_id, 0)).transpose(); + Eigen::Vector3d ori_wtoI = -180.0 / M_PI * ov_core::log_so3(e_R_wtoI); + + Eigen::Matrix3d e_R_atoI = ov_core::quat_2_Rot(gt_state.at(i).block<4,1>(atoI_id, 0)) * + ov_core::quat_2_Rot(est_state.at(i).block<4,1>(atoI_id, 0)).transpose(); + Eigen::Vector3d ori_atoI = -180.0 / M_PI * ov_core::log_so3(e_R_atoI); + + for(int j=0; j<3; j++){ + error_wtoI.at(j).timestamps.push_back(est_state.at(i)(0)); + error_wtoI.at(j).values.push_back(ori_wtoI(j)); + error_wtoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(wtoI_cov_id + j)); + error_atoI.at(j).timestamps.push_back(est_state.at(i)(0)); + error_atoI.at(j).values.push_back(ori_atoI(j)); + error_atoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(atoI_cov_id + j)); + } + + } + + // return if we don't want to plot + if (!doplotting) + return; + +#ifndef HAVE_PYTHONLIBS + PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); + return; +#else + + // Plot line colors + std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; + + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(0); + std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(1); + plot_3errors(error_dw[0], error_dw[1], error_dw[2], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Dw Error"); + matplotlibcpp::ylabel("dw_1"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("dw_2"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("dw_3"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_dw[3], error_dw[4], error_dw[5], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Dw Error"); + matplotlibcpp::ylabel("dw_3"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("dw_4"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("dw_5"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + //===================================================== + + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_da[0], error_da[1], error_da[2], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Da Error"); + matplotlibcpp::ylabel("da_1"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("da_2"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("da_3"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_da[3], error_da[4], error_da[5], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Da Error"); + matplotlibcpp::ylabel("da_3"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("da_4"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("da_5"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + //===================================================== + + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_tg[0], error_tg[1], error_tg[2], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::ylabel("tg_1"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("tg_2"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_3"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_tg[3], error_tg[4], error_tg[5], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::ylabel("tg_3"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("tg_4"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_5"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_tg[6], error_tg[7], error_tg[8], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::ylabel("tg_6"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("tg_7"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_8"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + //===================================================== + + if(imu_model == 0){ + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_wtoI[0], error_wtoI[1], error_wtoI[2], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU R_GyrotoI Error"); + matplotlibcpp::ylabel("x-error (deg)"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("y-error (deg)"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("z-error (deg)"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + //===================================================== + }else{ + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_atoI[0], error_atoI[1], error_atoI[2], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU R_AcctoI Error"); + matplotlibcpp::ylabel("x-error (deg)"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("y-error (deg)"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("z-error (deg)"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + //===================================================== + } + + + +#endif +} + diff --git a/ov_eval/src/calc/ResultSimulation.h b/ov_eval/src/calc/ResultSimulation.h index 0d3353d5a..3bb0b977e 100644 --- a/ov_eval/src/calc/ResultSimulation.h +++ b/ov_eval/src/calc/ResultSimulation.h @@ -95,6 +95,8 @@ class ResultSimulation { */ void plot_cam_extrinsics(bool doplotting, double max_time = INFINITY); + void plot_imu_intrinsics(bool doploting, double max_time = INFINITY); + protected: // Trajectory data (loaded from file and timestamp intersected) std::vector est_state, gt_state; diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp index 6a08c0fc0..05b8a0f97 100644 --- a/ov_eval/src/error_simulation.cpp +++ b/ov_eval/src/error_simulation.cpp @@ -34,39 +34,44 @@ int main(int argc, char **argv) { - // Verbosity setting - ov_core::Printer::setPrintLevel("INFO"); + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); - // Ensure we have a path - if (argc < 4) { - PRINT_ERROR(RED "ERROR: ./error_simulation \n" RESET); - PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation \n" RESET); - std::exit(EXIT_FAILURE); - } + // Ensure we have a path + if (argc < 4) { + PRINT_ERROR(RED "ERROR: ./error_simulation \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation \n" RESET); + std::exit(EXIT_FAILURE); + } - // Create our trajectory object - ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]); + // Create our trajectory object + ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]); - // Plot the state errors - PRINT_INFO("Plotting state variable errors...\n"); - traj.plot_state(true); + // Plot the state errors + PRINT_INFO("Plotting state variable errors...\n"); + traj.plot_state(true); - // Plot time offset - PRINT_INFO("Plotting time offset error...\n"); - traj.plot_timeoff(true, 10); + // Plot time offset + PRINT_INFO("Plotting time offset error...\n"); + traj.plot_timeoff(true, 10); - // Plot camera intrinsics - PRINT_INFO("Plotting camera intrinsics...\n"); - traj.plot_cam_instrinsics(true, 60); + // Plot camera intrinsics + PRINT_INFO("Plotting camera intrinsics...\n"); + traj.plot_cam_instrinsics(true, 60); - // Plot camera extrinsics - PRINT_INFO("Plotting camera extrinsics...\n"); - traj.plot_cam_extrinsics(true, 60); + // Plot camera extrinsics + PRINT_INFO("Plotting camera extrinsics...\n"); + traj.plot_cam_extrinsics(true, 60); + + + // Plot camera intrinsics + PRINT_INFO("Plotting IMU intrinsics...\n"); + traj.plot_imu_intrinsics(true); #ifdef HAVE_PYTHONLIBS - matplotlibcpp::show(true); + matplotlibcpp::show(true); #endif - // Done! - return EXIT_SUCCESS; + // Done! + return EXIT_SUCCESS; } From 714941f350db7cf383043d9e10abe87f19e18be6 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 09:26:46 -0800 Subject: [PATCH 06/60] add perturbation to simulation for imu intrinsics --- ov_msckf/src/sim/Simulator.cpp | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 3fa743c49..94a79cfa4 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -174,6 +174,33 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { params_.camera_extrinsics.at(i).block(0, 0, 4, 1) = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.camera_extrinsics.at(i).block(0, 0, 4, 1))); } + + // perturb the imu intrinsics + for(int j=0; j<6; j++){ + params_.imu_config.imu_x_dw(j) += 0.005 * w(gen_state_perturb); + params_.imu_config.imu_x_da(j) += 0.005 * w(gen_state_perturb); + } + + // if we need to perturb gravity sensitivity + for(int j=0; j<5; j++){ + if(params_.state_options.do_calib_imu_g_sensitivity){ + params_.imu_config.imu_x_tg(j) += 0.005 * w(gen_state_perturb); + } + } + + // depends on Kalibr model or RPNG model + if(params_.state_options.imu_model == 0){ + Eigen::Vector3d w_vec; + w_vec << 0.002 * w(gen_state_perturb), 0.002 * w(gen_state_perturb), 0.002 * w(gen_state_perturb); + params_.imu_config.imu_quat_GyrotoI = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.imu_quat_GyrotoI)); + }else{ + Eigen::Vector3d w_vec; + w_vec << 0.002 * w(gen_state_perturb), 0.002 * w(gen_state_perturb), 0.002 * w(gen_state_perturb); + params_.imu_config.imu_quat_AcctoI = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.imu_quat_AcctoI)); + } + + + } //=============================================================== From a8d48ece83478a6436a4c593c0869d518cdde74c Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 09:33:23 -0800 Subject: [PATCH 07/60] debuging and tesing the imu intrinsic calibrtion with sim --- ov_msckf/config/rpng_sim/estimator_config.yaml | 4 ++-- ov_msckf/launch/simulation.launch | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index de78b4e22..3a16c4afa 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -4,8 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: false # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 9d56eb33a..a16f5e068 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -16,7 +16,7 @@ - + @@ -24,8 +24,8 @@ - - + + From 78cda819184c047e244bd7c9c7d8c19af0e06974 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 10:59:40 -0800 Subject: [PATCH 08/60] debuging and testing ... --- .../config/rpng_sim/estimator_config.yaml | 4 +- .../config/rpng_sim/kalibr_imu_chain.yaml | 6 +- ov_msckf/launch/simulation.launch | 26 +++--- ov_msckf/src/ros/ROS1Visualizer.cpp | 5 ++ ov_msckf/src/sim/Simulator.cpp | 2 +- ov_msckf/src/state/Propagator.cpp | 83 ++++++++++++++++++- 6 files changed, 105 insertions(+), 21 deletions(-) diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index 3a16c4afa..de78b4e22 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -4,8 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: false # if analytic integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml index 7a7a40e15..a7558804b 100644 --- a/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml +++ b/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml @@ -30,7 +30,7 @@ imu0: - [0.0, 1.0, 0.0] - [0.0, 0.0, 1.0] Tg: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [0.0, 0.0, 0.0] + - [0.0, 0.0, 0.0] + - [0.0, 0.0, 0.0] model: kalibr \ No newline at end of file diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index a16f5e068..dd5447524 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -19,33 +19,35 @@ - + - - - - + + + + - - - - - + + + + + + + - - + + diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 469ac1869..485b2241d 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -634,6 +634,11 @@ void ROS1Visualizer::publish_groundtruth() { quat_diff = quat_multiply(quat_st, Inv(quat_gt)); double rmse_ori = (180 / M_PI) * 2 * quat_diff.block(0, 0, 3, 1).norm(); + // + std::cout << " debug the initial values " << std::endl; + std::cout << state_ekf.block<3,1>(4,0).transpose() << std::endl; + std::cout << state_gt.block<3,1>(5,0).transpose() << std::endl; + //========================================================================== //========================================================================== diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 94a79cfa4..bde49cc91 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -336,7 +336,7 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto Eigen::Matrix3d Tw = params.imu_config.Tw(); Eigen::Matrix3d Ta = params.imu_config.Ta(); Eigen::Vector3d omega_inw = Tw * params.imu_config.R_ItoGyro() * omega_inI + Tg * accel_inI; - Eigen::Vector3d accel_ina = Ta * params.imu_config.R_ItoAcc() * R_GtoI * accel_inI; + Eigen::Vector3d accel_ina = Ta * params.imu_config.R_ItoAcc() * accel_inI; diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 96adcaaaf..903183dc7 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -377,6 +377,11 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); +// std::cout << "check propagation: " << std::endl; +// std::cout << state->_imu->pos().transpose() << std::endl; +// std::cout << new_p.transpose() << std::endl; +// std::cout << a_hat.transpose() * dt << std::endl; + // Allocate state transition and noise Jacobian F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15,12); @@ -389,14 +394,86 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core new_q, new_v, new_p, F, G); } + +// // Get the locations of each entry of the imu state +// int th_id = state->_imu->q()->id() - state->_imu->id(); +// int p_id = state->_imu->p()->id() - state->_imu->id(); +// int v_id = state->_imu->v()->id() - state->_imu->id(); +// int bg_id = state->_imu->bg()->id() - state->_imu->id(); +// int ba_id = state->_imu->ba()->id() - state->_imu->id(); +// +// // Allocate noise Jacobian +//// Eigen::Matrix G = Eigen::Matrix::Zero(); +// +// // Now compute Jacobian of new state wrt old state and noise +// if (state->_options.do_fej) { +// +// // This is the change in the orientation from the end of the last prop to the current prop +// // This is needed since we need to include the "k-th" updated orientation information +// Eigen::Matrix Rfej = state->_imu->Rot_fej(); +// Eigen::Matrix dR = quat_2_Rot(new_q) * Rfej.transpose(); +// +// Eigen::Matrix v_fej = state->_imu->vel_fej(); +// Eigen::Matrix p_fej = state->_imu->pos_fej(); +// +// F.block(th_id, th_id, 3, 3) = dR; +// F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-w_hat * dt) * dt; +// // F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-log_so3(dR)) * dt; +// F.block(bg_id, bg_id, 3, 3).setIdentity(); +// F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v - v_fej + _gravity * dt) * Rfej.transpose(); +// // F.block(v_id, th_id, 3, 3).noalias() = -Rfej.transpose() * skew_x(Rfej*(new_v-v_fej+_gravity*dt)); +// F.block(v_id, v_id, 3, 3).setIdentity(); +// F.block(v_id, ba_id, 3, 3) = -Rfej.transpose() * dt; +// F.block(ba_id, ba_id, 3, 3).setIdentity(); +// F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p - p_fej - v_fej * dt + 0.5 * _gravity * dt * dt) * Rfej.transpose(); +// // F.block(p_id, th_id, 3, 3).noalias() = -0.5 * Rfej.transpose() * skew_x(2*Rfej*(new_p-p_fej-v_fej*dt+0.5*_gravity*dt*dt)); +// F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; +// F.block(p_id, ba_id, 3, 3) = -0.5 * Rfej.transpose() * dt * dt; +// F.block(p_id, p_id, 3, 3).setIdentity(); +// +// G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-w_hat * dt) * dt; +// // G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-log_so3(dR)) * dt; +// G.block(v_id, 3, 3, 3) = -Rfej.transpose() * dt; +// G.block(p_id, 3, 3, 3) = -0.5 * Rfej.transpose() * dt * dt; +// G.block(bg_id, 6, 3, 3) = Eigen::Matrix::Identity(); +// G.block(ba_id, 9, 3, 3) = Eigen::Matrix::Identity(); +// +// } else { +// +// Eigen::Matrix R_Gtoi = state->_imu->Rot(); +// +// F.block(th_id, th_id, 3, 3) = exp_so3(-w_hat * dt); +// F.block(th_id, bg_id, 3, 3).noalias() = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; +// F.block(bg_id, bg_id, 3, 3).setIdentity(); +// F.block(v_id, th_id, 3, 3).noalias() = -R_Gtoi.transpose() * skew_x(a_hat * dt); +// F.block(v_id, v_id, 3, 3).setIdentity(); +// F.block(v_id, ba_id, 3, 3) = -R_Gtoi.transpose() * dt; +// F.block(ba_id, ba_id, 3, 3).setIdentity(); +// F.block(p_id, th_id, 3, 3).noalias() = -0.5 * R_Gtoi.transpose() * skew_x(a_hat * dt * dt); +// F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; +// F.block(p_id, ba_id, 3, 3) = -0.5 * R_Gtoi.transpose() * dt * dt; +// F.block(p_id, p_id, 3, 3).setIdentity(); +// +// G.block(th_id, 0, 3, 3) = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; +// G.block(v_id, 3, 3, 3) = -R_Gtoi.transpose() * dt; +// G.block(p_id, 3, 3, 3) = -0.5 * R_Gtoi.transpose() * dt * dt; +// G.block(bg_id, 6, 3, 3) = Eigen::Matrix::Identity(); +// G.block(ba_id, 9, 3, 3) = Eigen::Matrix::Identity(); +// } + + + + + + // Construct our discrete noise covariance matrix // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report Eigen::Matrix Qc = Eigen::Matrix::Zero(); Qc.block(0, 0, 3, 3) = _imu_config.sigma_w_2 / dt * Eigen::Matrix::Identity(); Qc.block(3, 3, 3, 3) = _imu_config.sigma_a_2 / dt * Eigen::Matrix::Identity(); - Qc.block(6, 6, 3, 3) = _imu_config.sigma_wb_2 * dt * Eigen::Matrix::Identity(); - Qc.block(9, 9, 3, 3) = _imu_config.sigma_ab_2 * dt * Eigen::Matrix::Identity(); + Qc.block(6, 6, 3, 3) = _imu_config.sigma_wb_2 / dt * Eigen::Matrix::Identity(); + Qc.block(9, 9, 3, 3) = _imu_config.sigma_ab_2 / dt * Eigen::Matrix::Identity(); // Compute the noise injected into the state over the interval Qd = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); @@ -759,7 +836,7 @@ void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, Eigen::Matrix R_Gtok = state->_imu->Rot(); // get the pre-computed value - Eigen::Matrix3d R_k1_to_k = Xi_sum.block(0,0,3,3); + Eigen::Matrix3d R_k1_to_k = Xi_sum.block<3,3>(0,0); Eigen::Matrix3d Xi_1 = Xi_sum.block<3,3>(0,3); Eigen::Matrix3d Xi_2 = Xi_sum.block<3,3>(0,6); From 694f9323cb4b708ecb5bdaafc6471d7e26b85b55 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 11:15:54 -0800 Subject: [PATCH 09/60] the intrinsic calibration with analytic jacobians is done --- ov_eval/src/calc/ResultSimulation.cpp | 5 +++-- ov_msckf/launch/simulation.launch | 2 +- ov_msckf/src/sim/Simulator.cpp | 6 +++--- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index b3282354f..0363138c1 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -508,7 +508,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { return; } - int num_cam = 1 + 16 + 1; + int num_cam = (int)est_state.at(0)(18); int imu_model = (int) est_state.at(0)(1 + 16 + 1 + 1 + 15 * num_cam); int dw_id = 1 + 16 + 1 + 1 + 15 * num_cam + 1; int dw_cov_id = 1 + 15 + 1 + 1 + 14 * num_cam + 1; @@ -521,6 +521,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { int atoI_id = wtoI_id + 4; int atoI_cov_id = wtoI_cov_id + 3; + // Camera extrinsics statistic storage std::vector error_dw, error_da, error_tg, error_wtoI, error_atoI; @@ -536,7 +537,6 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { error_atoI.emplace_back(Statistics()); } - // Loop through and calculate error double start_time = est_state.at(0)(0); for (size_t i = 0; i < est_state.size(); i++) { @@ -591,6 +591,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { } + // return if we don't want to plot if (!doplotting) return; diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index dd5447524..f7b4f35ec 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -10,7 +10,7 @@ - + diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index bde49cc91..ca4eeebb7 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -177,14 +177,14 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // perturb the imu intrinsics for(int j=0; j<6; j++){ - params_.imu_config.imu_x_dw(j) += 0.005 * w(gen_state_perturb); - params_.imu_config.imu_x_da(j) += 0.005 * w(gen_state_perturb); + params_.imu_config.imu_x_dw(j) += 0.004 * w(gen_state_perturb); + params_.imu_config.imu_x_da(j) += 0.004 * w(gen_state_perturb); } // if we need to perturb gravity sensitivity for(int j=0; j<5; j++){ if(params_.state_options.do_calib_imu_g_sensitivity){ - params_.imu_config.imu_x_tg(j) += 0.005 * w(gen_state_perturb); + params_.imu_config.imu_x_tg(j) += 0.004 * w(gen_state_perturb); } } From 9f5a642aa5dff83bb9ba09a0541b4565eb002a9b Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 11:57:39 -0800 Subject: [PATCH 10/60] modify the config --- ov_eval/src/error_simulation.cpp | 2 +- .../config/euroc_mav/estimator_config.yaml | 2 ++ .../config/euroc_mav/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- ov_msckf/config/kaist/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- .../config/kaist_vio/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- .../config/rpng_aruco/estimator_config.yaml | 2 ++ .../config/rpng_aruco/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- .../rpng_ironsides/estimator_config.yaml | 2 ++ .../rpng_ironsides/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- .../config/rpng_sim/estimator_config.yaml | 6 +++-- ov_msckf/config/tum_vi/estimator_config.yaml | 2 ++ ov_msckf/config/tum_vi/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- .../uzhfpv_indoor/estimator_config.yaml | 2 ++ .../uzhfpv_indoor/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- .../uzhfpv_indoor_45/estimator_config.yaml | 2 ++ .../uzhfpv_indoor_45/kalibr_imu_chain.yaml | 24 +++++++++++++++++-- ov_msckf/launch/simulation.launch | 6 ++--- 17 files changed, 196 insertions(+), 22 deletions(-) diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp index 05b8a0f97..62eb8d0a4 100644 --- a/ov_eval/src/error_simulation.cpp +++ b/ov_eval/src/error_simulation.cpp @@ -64,7 +64,7 @@ int main(int argc, char **argv) { traj.plot_cam_extrinsics(true, 60); - // Plot camera intrinsics + // Plot IMU intrinsics PRINT_INFO("Plotting IMU intrinsics...\n"); traj.plot_imu_intrinsics(true); diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index 31d72b158..0fbe71084 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: true calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 11 max_slam: 50 diff --git a/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml b/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml index 394cdca13..635017056 100644 --- a/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml +++ b/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated + model: kalibr rostopic: /imu0 time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/ov_msckf/config/kaist/kalibr_imu_chain.yaml b/ov_msckf/config/kaist/kalibr_imu_chain.yaml index 6fdd0ef0a..f1ed900e3 100644 --- a/ov_msckf/config/kaist/kalibr_imu_chain.yaml +++ b/ov_msckf/config/kaist/kalibr_imu_chain.yaml @@ -13,7 +13,27 @@ imu0: accelerometer_random_walk: 1.0000e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.7453e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.0000e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated + model: kalibr rostopic: /imu/data_raw time_offset: 0.0 - update_rate: 500.0 \ No newline at end of file + update_rate: 500.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml b/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml index 8bec50983..5bf3f5f3e 100644 --- a/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml +++ b/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 0.00047402 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 0.00005770 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 0.00001565 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /mavros/imu/data time_offset: 0.0 - update_rate: 100.0 \ No newline at end of file + update_rate: 100.0 + Tw: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + R_ItoGyro: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + Ta: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + R_ItoAcc: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + Tg: + - [0.0, 0.0, 0.0] + - [0.0, 0.0, 0.0] + - [0.0, 0.0, 0.0] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/ov_msckf/config/rpng_aruco/estimator_config.yaml index 48ad03e9c..a170a5356 100644 --- a/ov_msckf/config/rpng_aruco/estimator_config.yaml +++ b/ov_msckf/config/rpng_aruco/estimator_config.yaml @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: true calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 15 max_slam: 75 diff --git a/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml index 394cdca13..462154309 100644 --- a/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml +++ b/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /imu0 time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/ov_msckf/config/rpng_ironsides/estimator_config.yaml index 36bfe2759..abe432bc9 100644 --- a/ov_msckf/config/rpng_ironsides/estimator_config.yaml +++ b/ov_msckf/config/rpng_ironsides/estimator_config.yaml @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: false calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 12 max_slam: 50 diff --git a/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml index 7e14da487..38a1d063e 100644 --- a/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml +++ b/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 1.3054568211204843e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.1186830841306218e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 8.997530210630026e-07 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /imu0 time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index de78b4e22..a606f8fef 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -4,8 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: false # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: true calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: true +calib_imu_g_sensitivity: true max_clones: 11 max_slam: 50 diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml index d122789b7..32808a663 100644 --- a/ov_msckf/config/tum_vi/estimator_config.yaml +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: true calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 11 max_slam: 50 diff --git a/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml b/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml index 8571eeb9f..942c3808a 100644 --- a/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml +++ b/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 0.00086 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 0.00016 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 2.2e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /imu0 time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml index b6ce6db3a..eb8f2398a 100644 --- a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: false calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 11 max_slam: 50 diff --git a/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml index 5e91ccd79..d3e763130 100644 --- a/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -14,7 +14,27 @@ imu0: # accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) # gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) # gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /snappy_imu time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml index b6ce6db3a..eb8f2398a 100644 --- a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml @@ -13,6 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: false calib_cam_intrinsics: true calib_cam_timeoffset: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 11 max_slam: 50 diff --git a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml index 32c182c76..c599f122d 100644 --- a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml +++ b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -14,7 +14,27 @@ imu0: # accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) # gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) # gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated rostopic: /snappy_imu time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file + update_rate: 200.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + model: kalibr \ No newline at end of file diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index f7b4f35ec..580e35f79 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -19,7 +19,7 @@ - + @@ -46,8 +46,8 @@ - - + + From 7261b756cc3051486a54c98c8d2409ea7a8f8b6d Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 13:12:16 -0800 Subject: [PATCH 11/60] add comments --- config/rpng_sim/estimator_config.yaml | 4 +- ov_msckf/src/state/Propagator.h | 106 +++++++++++++++++++++----- 2 files changed, 90 insertions(+), 20 deletions(-) diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index 077a3a1a8..f59932c06 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -4,8 +4,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: false # if analytic integration should be used (overrides imu averaging) +use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 1c291a5ac..81c387a3f 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -225,20 +225,59 @@ class Propagator { Eigen::Vector3d &new_p); - /// TODO: comment this.... + /** + * @brief Analytically compute the integration componenets based on ACI^2 + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param Xi_sum All the needed integration componenets, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + */ + void compute_Xi_sum(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Matrix &Xi_sum); + + + /** + * @brief Analytically predict IMU mean + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param Xi_sum All the needed integration componenets, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + */ void predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, Eigen::Matrix &Xi_sum); - - /// TODO: comment this.... - void compute_Xi_sum(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Matrix &Xi_sum); - - /// TODO: comment this.... + /** + * @brief Analytically compute state transition matrix F and noise Jacobian G + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param Xi_sum All the needed integration componenets, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + * @param F State transition matrix + * @param G Noise Jacobian + */ void compute_F_and_G_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, @@ -246,27 +285,58 @@ class Propagator { const Eigen::Matrix &Xi_sum, Eigen::MatrixXd &F, Eigen::MatrixXd &G); - /// TODO: comment this.... + /** + * @brief compute state transition matrix F and noise Jacobian G + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param F State transition matrix + * @param G Noise Jacobian + */ void compute_F_and_G_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, Eigen::MatrixXd &F, Eigen::MatrixXd &G); - /// TODO: comment this.... + /** + * @brief compute the Jacobians for Dw + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed + */ Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); - /// TODO: comment this.... + /** + * @brief compute the Jacobians for Da + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param a_uncorrected Linear acceleration in gyro frame with bias removed + */ Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); - /// TODO: comment this.... + /** + * @brief compute the Jacobians for Tg + * + * See the paper for ACI^2 or IJRR paper + * + * @param state Pointer to state + * @param a_inI Linear acceleration with bias removed + */ Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); - - - - - - + /// Container for the noise values ov_core::ImuConfig _imu_config; From 7fbf21279aa334a892a25b6da0b948e9056be292 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 13:14:26 -0800 Subject: [PATCH 12/60] make the launch file good to go --- ov_msckf/launch/simulation.launch | 11 ++++------- ov_msckf/src/state/Propagator.h | 2 +- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index c9acb8b2c..e8a30b25c 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -33,13 +33,10 @@ - - - - - - - + + + + diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 81c387a3f..737af113d 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -336,7 +336,7 @@ class Propagator { * @param a_inI Linear acceleration with bias removed */ Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); - + /// Container for the noise values ov_core::ImuConfig _imu_config; From 3dec739a5a0867b594120faea186982df836a892 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Tue, 14 Dec 2021 14:02:42 -0800 Subject: [PATCH 13/60] increase the prior of cam-imu calibration --- config/rpng_sim/estimator_config.yaml | 4 ++-- config/rpng_sim/kalibr_imu_chain.yaml | 2 +- ov_msckf/launch/simulation.launch | 4 +++- ov_msckf/src/state/State.cpp | 2 +- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index f59932c06..5071a8361 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -13,8 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: true calib_cam_intrinsics: true calib_cam_timeoffset: true -calib_imu_intrinsics: true -calib_imu_g_sensitivity: true +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false max_clones: 11 max_slam: 50 diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index a7558804b..4645b1593 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -33,4 +33,4 @@ imu0: - [0.0, 0.0, 0.0] - [0.0, 0.0, 0.0] - [0.0, 0.0, 0.0] - model: kalibr \ No newline at end of file + model: rpng \ No newline at end of file diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index e8a30b25c..e2e12ee55 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -10,7 +10,7 @@ - + @@ -38,6 +38,8 @@ + + diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index e75cea930..373635d85 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -136,7 +136,7 @@ State::State(StateOptions &options) { for (int i = 0; i < _options.num_cameras; i++) { _Cov.block(_calib_IMUtoCAM.at(i)->id(), _calib_IMUtoCAM.at(i)->id(), 3, 3) = std::pow(0.001, 2) * Eigen::MatrixXd::Identity(3, 3); _Cov.block(_calib_IMUtoCAM.at(i)->id() + 3, _calib_IMUtoCAM.at(i)->id() + 3, 3, 3) = - std::pow(0.01, 2) * Eigen::MatrixXd::Identity(3, 3); + std::pow(0.015, 2) * Eigen::MatrixXd::Identity(3, 3); } } if (_options.do_calib_camera_intrinsics) { From 8333026921073b52ce1f3abd25fa5e3bf1f82de0 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Sat, 18 Dec 2021 16:24:53 -0800 Subject: [PATCH 14/60] fix comments --- ov_eval/src/calc/ResultSimulation.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 0363138c1..7a2361d30 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -508,6 +508,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { return; } + // get the parameters id int num_cam = (int)est_state.at(0)(18); int imu_model = (int) est_state.at(0)(1 + 16 + 1 + 1 + 15 * num_cam); int dw_id = 1 + 16 + 1 + 1 + 15 * num_cam + 1; @@ -522,7 +523,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { int atoI_cov_id = wtoI_cov_id + 3; - // Camera extrinsics statistic storage + // IMU intrinsics statistic storage std::vector error_dw, error_da, error_tg, error_wtoI, error_atoI; for (int j = 0; j < 6; j++) { @@ -714,6 +715,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { //===================================================== if(imu_model == 0){ + // Kalibr model //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 500); @@ -731,6 +733,7 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { matplotlibcpp::show(false); //===================================================== }else{ + // RPNG model //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 500); From fd92e1c2bda658ce20b064c98231b7cb8a98e26e Mon Sep 17 00:00:00 2001 From: yangyulin Date: Sat, 18 Dec 2021 17:45:00 -0800 Subject: [PATCH 15/60] fix comments and update the config files --- config/euroc_mav/estimator_config.yaml | 2 + config/euroc_mav/kalibr_imu_chain.yaml | 2 +- config/kaist/kalibr_imu_chain.yaml | 24 +- config/kaist_vio/kalibr_imu_chain.yaml | 2 +- config/rpng_aruco/kalibr_imu_chain.yaml | 2 +- config/rpng_ironsides/kalibr_imu_chain.yaml | 2 +- config/rpng_sim/estimator_config.yaml | 4 +- config/rpng_sim/kalibr_imu_chain.yaml | 2 +- config/tum_vi/kalibr_imu_chain.yaml | 2 +- config/uzhfpv_indoor/kalibr_imu_chain.yaml | 2 +- config/uzhfpv_indoor_45/kalibr_imu_chain.yaml | 2 +- ov_core/src/utils/sensor_data.h | 18 +- ov_msckf/src/core/VioManagerOptions.h | 6 +- ov_msckf/src/state/Propagator.cpp | 259 +++++++----------- ov_msckf/src/state/State.cpp | 12 +- 15 files changed, 165 insertions(+), 176 deletions(-) diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index 98401ddd1..ae2b7540a 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -12,6 +12,8 @@ max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular ( calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated +calib_imu_g_sensitivity: false # if gravity sensitivity should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 50 # number of features in our state vector diff --git a/config/euroc_mav/kalibr_imu_chain.yaml b/config/euroc_mav/kalibr_imu_chain.yaml index 635017056..7825b7afe 100644 --- a/config/euroc_mav/kalibr_imu_chain.yaml +++ b/config/euroc_mav/kalibr_imu_chain.yaml @@ -10,7 +10,7 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: kalibr + model: kalibr # 0: kalibr and 1: rpng rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 diff --git a/config/kaist/kalibr_imu_chain.yaml b/config/kaist/kalibr_imu_chain.yaml index b3eea522e..6bd3c3213 100644 --- a/config/kaist/kalibr_imu_chain.yaml +++ b/config/kaist/kalibr_imu_chain.yaml @@ -17,7 +17,27 @@ imu0: # # # - model: calibrated rostopic: /imu/data_raw time_offset: 0.0 - update_rate: 500.0 \ No newline at end of file + update_rate: 500.0 + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoGyro: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_ItoAcc: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/kaist_vio/kalibr_imu_chain.yaml b/config/kaist_vio/kalibr_imu_chain.yaml index 5bf3f5f3e..0d33e793d 100644 --- a/config/kaist_vio/kalibr_imu_chain.yaml +++ b/config/kaist_vio/kalibr_imu_chain.yaml @@ -33,4 +33,4 @@ imu0: - [0.0, 0.0, 0.0] - [0.0, 0.0, 0.0] - [0.0, 0.0, 0.0] - model: kalibr \ No newline at end of file + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/rpng_aruco/kalibr_imu_chain.yaml b/config/rpng_aruco/kalibr_imu_chain.yaml index 462154309..29721bd37 100644 --- a/config/rpng_aruco/kalibr_imu_chain.yaml +++ b/config/rpng_aruco/kalibr_imu_chain.yaml @@ -33,4 +33,4 @@ imu0: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - model: kalibr \ No newline at end of file + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/rpng_ironsides/kalibr_imu_chain.yaml b/config/rpng_ironsides/kalibr_imu_chain.yaml index 38a1d063e..fee670bc0 100644 --- a/config/rpng_ironsides/kalibr_imu_chain.yaml +++ b/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -33,4 +33,4 @@ imu0: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - model: kalibr \ No newline at end of file + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index 5071a8361..f59932c06 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -13,8 +13,8 @@ max_cameras: 2 calib_cam_extrinsics: true calib_cam_intrinsics: true calib_cam_timeoffset: true -calib_imu_intrinsics: false -calib_imu_g_sensitivity: false +calib_imu_intrinsics: true +calib_imu_g_sensitivity: true max_clones: 11 max_slam: 50 diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 4645b1593..45ce13f9f 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -33,4 +33,4 @@ imu0: - [0.0, 0.0, 0.0] - [0.0, 0.0, 0.0] - [0.0, 0.0, 0.0] - model: rpng \ No newline at end of file + model: rpng # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/tum_vi/kalibr_imu_chain.yaml b/config/tum_vi/kalibr_imu_chain.yaml index 942c3808a..b7dfeadcd 100644 --- a/config/tum_vi/kalibr_imu_chain.yaml +++ b/config/tum_vi/kalibr_imu_chain.yaml @@ -33,4 +33,4 @@ imu0: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - model: kalibr \ No newline at end of file + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/config/uzhfpv_indoor/kalibr_imu_chain.yaml index d3e763130..3dd4b198f 100644 --- a/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -37,4 +37,4 @@ imu0: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - model: kalibr \ No newline at end of file + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml index c599f122d..a192a592c 100644 --- a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -37,4 +37,4 @@ imu0: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - model: kalibr \ No newline at end of file + model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index 5ec71eada..677ca0282 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -86,11 +86,11 @@ struct ImuConfig { int imu_model = 0; /// imu intrinsics - Eigen::Matrix imu_x_dw; // the columnwise elements for Dw - Eigen::Matrix imu_x_da; // the columnwise elements for Da - Eigen::Matrix imu_x_tg; // the ccolumnwise elements for Tg - Eigen::Matrix imu_quat_AcctoI; // the JPL quat for R_AcctoI - Eigen::Matrix imu_quat_GyrotoI; // the JPL quat for R_GyrotoI + Eigen::Matrix imu_x_dw; /// the columnwise elements for Dw + Eigen::Matrix imu_x_da; /// the columnwise elements for Da + Eigen::Matrix imu_x_tg; /// the ccolumnwise elements for Tg + Eigen::Matrix imu_quat_AcctoI; /// the JPL quat for R_AcctoI + Eigen::Matrix imu_quat_GyrotoI; /// the JPL quat for R_GyrotoI /// Gyroscope white noise (rad/s/sqrt(hz)) double sigma_w = 1.6968e-04; @@ -128,11 +128,13 @@ struct ImuConfig { Eigen::Matrix3d Dw(){ Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); if(imu_model == 0){ + // Kalibr model with lower triangular of the matrix Dw << imu_x_dw(0), 0, 0, imu_x_dw(1), imu_x_dw(3), 0, imu_x_dw(2), imu_x_dw(4), imu_x_dw(5); }else{ + // rpng model with upper triangular of the matrix Dw << imu_x_dw(0), imu_x_dw(1), imu_x_dw(3), 0, imu_x_dw(2), imu_x_dw(4), @@ -145,11 +147,13 @@ struct ImuConfig { Eigen::Matrix3d Da(){ Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); if(imu_model == 0){ + // kalibr model with lower triangular of the matrix Da << imu_x_da(0), 0, 0, imu_x_da(1), imu_x_da(3), 0, imu_x_da(2), imu_x_da(4), imu_x_da(5); }else{ + // rpng model with upper triangular of the matrix Da << imu_x_da(0), imu_x_da(1), imu_x_da(3), 0, imu_x_da(2), imu_x_da(4), @@ -163,6 +167,7 @@ struct ImuConfig { return Dw().inverse(); } + /// get the IMU Ta Eigen::Matrix3d Ta() { return Da().inverse(); } @@ -180,7 +185,7 @@ struct ImuConfig { Eigen::Matrix3d R_AcctoI(){ return ov_core::quat_2_Rot(imu_quat_AcctoI); } - + /// get the R_ItoAcc Eigen::Matrix3d R_ItoAcc(){ return R_AcctoI().transpose(); } @@ -190,6 +195,7 @@ struct ImuConfig { return ov_core::quat_2_Rot(imu_quat_GyrotoI); } + /// get the R_ItoGyro Eigen::Matrix3d R_ItoGyro(){ return R_GyrotoI().transpose(); } diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 9ed2daa90..9f6649070 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -312,15 +312,19 @@ struct VioManagerOptions { // store these parameters if(imu_config.imu_model == 0){ + // kalibr model + // lower triangular of the matrix imu_config.imu_x_dw << Dw.block<3,1>(0,0), Dw.block<2,1>(1,1), Dw(2,2); imu_config.imu_x_da << Da.block<3,1>(0,0), Da.block<2,1>(1,1), Da(2,2); }else{ + // rpng model + // upper triangular of the matrix imu_config.imu_x_dw << Dw(0,0), Dw.block<2,1>(0,1), Dw.block<3,1>(0,2); imu_config.imu_x_da << Da(0,0), Da.block<2,1>(0,1), Da.block<3,1>(0,2); } + imu_config.imu_x_tg << Tg.block<3,1>(0,0), Tg.block<3,1>(0,1), Tg.block<3,1>(0,2); imu_config.imu_quat_GyrotoI = ov_core::rot_2_quat(R_GyrotoI); imu_config.imu_quat_AcctoI = ov_core::rot_2_quat(R_AcctoI); - imu_config.imu_x_tg << Tg.block<3,1>(0,0), Tg.block<3,1>(0,1), Tg.block<3,1>(0,2); } PRINT_DEBUG("STATE PARAMETERS:\n"); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 903183dc7..d2b12f443 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -157,19 +157,56 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times for (size_t i = 0; i < prop_data.size() - 1; i++) { // Time elapsed over interval - double dt = prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp; + auto data_plus = prop_data.at(i + 1); + auto data_minus = prop_data.at(i); + double dt = data_plus.timestamp - data_minus.timestamp; // assert(data_plus.timestamp>data_minus.timestamp); - // Corrected imu measurements - Eigen::Matrix w_hat = prop_data.at(i).wm - state->_imu->bias_g(); - Eigen::Matrix a_hat = prop_data.at(i).am - state->_imu->bias_a(); - Eigen::Matrix w_hat2 = prop_data.at(i + 1).wm - state->_imu->bias_g(); - Eigen::Matrix a_hat2 = prop_data.at(i + 1).am - state->_imu->bias_a(); + // Corrected imu acc measurements with our current biases + Eigen::Matrix a_hat1 = data_minus.am - state->_imu->bias_a(); + Eigen::Matrix a_hat2 = data_plus.am - state->_imu->bias_a(); + Eigen::Matrix a_hat = a_hat1; + if (state->_options.imu_avg) { + a_hat = .5*(a_hat1+a_hat2); + } + // Imu acc readings in raw frame + Eigen::Vector3d a_uncorrected = a_hat; + // Correct imu readings with IMU intrinsics + a_hat = state->R_AcctoI() * state->Da() * a_hat; + a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; + a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; + + // gravity sensitivity correction + Eigen::Matrix gravity_correction1 = state->Tg() * a_hat1; + Eigen::Matrix gravity_correction2 = state->Tg() * a_hat2; + + // Corrected imu gyro measurements with our current biases + Eigen::Matrix w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; + Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; + Eigen::Matrix w_hat = w_hat1; + if (state->_options.imu_avg) { + w_hat = .5*(w_hat1+w_hat2); + } + // Imu readings in raw frame + Eigen::Vector3d w_uncorrected = w_hat; + + // Correct imu readings with IMU intrinsics + w_hat = state->R_GyrotoI() * state->Dw() * w_hat; + w_hat1 = state->R_GyrotoI() * state->Dw() * w_hat1; + w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; + + // Pre-compute some analytical values + Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3,18); + if (state->_options.use_analytic_integration) { + compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); + } // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if (state->_options.use_rk4_integration) + if(state->_options.use_analytic_integration) + predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); + else if (state->_options.use_rk4_integration) predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); else predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); @@ -376,17 +413,11 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core else predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); - -// std::cout << "check propagation: " << std::endl; -// std::cout << state->_imu->pos().transpose() << std::endl; -// std::cout << new_p.transpose() << std::endl; -// std::cout << a_hat.transpose() * dt << std::endl; - // Allocate state transition and noise Jacobian F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15,12); - if(state->_options.use_analytic_integration){ + if(state->_options.use_analytic_integration || state->_options.use_rk4_integration){ compute_F_and_G_analytic(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G); } else { @@ -394,78 +425,6 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core new_q, new_v, new_p, F, G); } - -// // Get the locations of each entry of the imu state -// int th_id = state->_imu->q()->id() - state->_imu->id(); -// int p_id = state->_imu->p()->id() - state->_imu->id(); -// int v_id = state->_imu->v()->id() - state->_imu->id(); -// int bg_id = state->_imu->bg()->id() - state->_imu->id(); -// int ba_id = state->_imu->ba()->id() - state->_imu->id(); -// -// // Allocate noise Jacobian -//// Eigen::Matrix G = Eigen::Matrix::Zero(); -// -// // Now compute Jacobian of new state wrt old state and noise -// if (state->_options.do_fej) { -// -// // This is the change in the orientation from the end of the last prop to the current prop -// // This is needed since we need to include the "k-th" updated orientation information -// Eigen::Matrix Rfej = state->_imu->Rot_fej(); -// Eigen::Matrix dR = quat_2_Rot(new_q) * Rfej.transpose(); -// -// Eigen::Matrix v_fej = state->_imu->vel_fej(); -// Eigen::Matrix p_fej = state->_imu->pos_fej(); -// -// F.block(th_id, th_id, 3, 3) = dR; -// F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-w_hat * dt) * dt; -// // F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-log_so3(dR)) * dt; -// F.block(bg_id, bg_id, 3, 3).setIdentity(); -// F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v - v_fej + _gravity * dt) * Rfej.transpose(); -// // F.block(v_id, th_id, 3, 3).noalias() = -Rfej.transpose() * skew_x(Rfej*(new_v-v_fej+_gravity*dt)); -// F.block(v_id, v_id, 3, 3).setIdentity(); -// F.block(v_id, ba_id, 3, 3) = -Rfej.transpose() * dt; -// F.block(ba_id, ba_id, 3, 3).setIdentity(); -// F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p - p_fej - v_fej * dt + 0.5 * _gravity * dt * dt) * Rfej.transpose(); -// // F.block(p_id, th_id, 3, 3).noalias() = -0.5 * Rfej.transpose() * skew_x(2*Rfej*(new_p-p_fej-v_fej*dt+0.5*_gravity*dt*dt)); -// F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; -// F.block(p_id, ba_id, 3, 3) = -0.5 * Rfej.transpose() * dt * dt; -// F.block(p_id, p_id, 3, 3).setIdentity(); -// -// G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-w_hat * dt) * dt; -// // G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-log_so3(dR)) * dt; -// G.block(v_id, 3, 3, 3) = -Rfej.transpose() * dt; -// G.block(p_id, 3, 3, 3) = -0.5 * Rfej.transpose() * dt * dt; -// G.block(bg_id, 6, 3, 3) = Eigen::Matrix::Identity(); -// G.block(ba_id, 9, 3, 3) = Eigen::Matrix::Identity(); -// -// } else { -// -// Eigen::Matrix R_Gtoi = state->_imu->Rot(); -// -// F.block(th_id, th_id, 3, 3) = exp_so3(-w_hat * dt); -// F.block(th_id, bg_id, 3, 3).noalias() = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; -// F.block(bg_id, bg_id, 3, 3).setIdentity(); -// F.block(v_id, th_id, 3, 3).noalias() = -R_Gtoi.transpose() * skew_x(a_hat * dt); -// F.block(v_id, v_id, 3, 3).setIdentity(); -// F.block(v_id, ba_id, 3, 3) = -R_Gtoi.transpose() * dt; -// F.block(ba_id, ba_id, 3, 3).setIdentity(); -// F.block(p_id, th_id, 3, 3).noalias() = -0.5 * R_Gtoi.transpose() * skew_x(a_hat * dt * dt); -// F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; -// F.block(p_id, ba_id, 3, 3) = -0.5 * R_Gtoi.transpose() * dt * dt; -// F.block(p_id, p_id, 3, 3).setIdentity(); -// -// G.block(th_id, 0, 3, 3) = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; -// G.block(v_id, 3, 3, 3) = -R_Gtoi.transpose() * dt; -// G.block(p_id, 3, 3, 3) = -0.5 * R_Gtoi.transpose() * dt * dt; -// G.block(bg_id, 6, 3, 3) = Eigen::Matrix::Identity(); -// G.block(ba_id, 9, 3, 3) = Eigen::Matrix::Identity(); -// } - - - - - - // Construct our discrete noise covariance matrix // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report @@ -533,7 +492,6 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d } } - //============================================================ //============================================================ @@ -550,7 +508,6 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d } Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q)*R_k.transpose(); - Eigen::Matrix Da = state->Da(); Eigen::Matrix Dw = state->Dw(); Eigen::Matrix Tg = state->Tg(); @@ -566,33 +523,32 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Eigen::Matrix3d Xi_4 = Xi_sum.block<3,3>(0,15); // for th - F.block(th_id, th_id, 3, 3) = dR_ktok1; - F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); - F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); + F.block<3,3>(th_id, th_id) = dR_ktok1; + F.block<3,3>(p_id, th_id).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); + F.block<3,3>(v_id, th_id).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); // for p - F.block(p_id, p_id, 3, 3).setIdentity(); + F.block<3,3>(p_id, p_id).setIdentity(); // for v - F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; - F.block(v_id, v_id, 3, 3).setIdentity(); + F.block<3,3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; + F.block<3,3>(v_id, v_id).setIdentity(); // for bg - F.block(th_id, bg_id, 3, 3).noalias() = -Jr * dt * R_wtoI * Dw; - F.block(p_id, bg_id, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI* Dw; - F.block(v_id, bg_id, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI* Dw; - F.block(bg_id, bg_id, 3, 3).setIdentity(); + F.block<3,3>(th_id, bg_id).noalias() = -Jr * dt * R_wtoI * Dw; + F.block<3,3>(p_id, bg_id) = R_k.transpose() * Xi_4 * R_wtoI* Dw; + F.block<3,3>(v_id, bg_id) = R_k.transpose() * Xi_3 * R_wtoI* Dw; + F.block<3,3>(bg_id, bg_id).setIdentity(); // for ba - F.block(th_id, ba_id, 3, 3).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - F.block(p_id, ba_id, 3, 3) = -R_k.transpose()*(Xi_2+Xi_4*R_wtoI*Dw*Tg)*R_atoI*Da; - F.block(v_id, ba_id, 3, 3) = -R_k.transpose()*(Xi_1+Xi_3*R_wtoI*Dw*Tg)*R_atoI*Da; - F.block(ba_id, ba_id, 3, 3).setIdentity(); + F.block<3,3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block<3,3>(p_id, ba_id) = -R_k.transpose()*(Xi_2+Xi_4*R_wtoI*Dw*Tg)*R_atoI*Da; + F.block<3,3>(v_id, ba_id) = -R_k.transpose()*(Xi_1+Xi_3*R_wtoI*Dw*Tg)*R_atoI*Da; + F.block<3,3>(ba_id, ba_id).setIdentity(); // begin to add the state transition matrix for the omega intrinsics part if(Dw_id != -1){ - // F.block(Dw_id, Dw_id, state->get_imu_intrinsics_gyro()->size(), state->get_imu_intrinsics_gyro()->size()).setIdentity(); F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); F.block(p_id, Dw_id, 3, state->_imu_x_dw->size()) = - R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state,w_uncorrected); F.block(v_id, Dw_id, 3, state->_imu_x_dw->size()) = - R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state,w_uncorrected); @@ -607,6 +563,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg )* R_atoI * compute_H_Da(state,a_uncorrected); } + // add the state trasition matrix of the tg part if(Tg_id != -1){ F.block(Tg_id,Tg_id,state->_imu_x_tg->size(),state->_imu_x_tg->size()).setIdentity(); F.block(th_id,Tg_id,3,state->_imu_x_tg->size()) = -Jr*dt*R_wtoI*Dw*compute_H_Tg(state,a_k); @@ -614,32 +571,31 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d F.block(v_id,Tg_id,3,state->_imu_x_tg->size())=R_k.transpose()*Xi_3*R_wtoI*Dw*compute_H_Tg(state,a_k); } - // begin to add the state transition matrix for the acc intrinsics part + // begin to add the state transition matrix for the acctoI part if(th_atoI_id != -1){ - F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); - F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); - F.block(p_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); - F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); + F.block<3,3>(th_atoI_id, th_atoI_id).setIdentity(); + F.block<3,3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block<3,3>(p_id, th_atoI_id) = R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); + F.block<3,3>(v_id, th_atoI_id) = R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); } - + // begin to add the state transition matrix for the gyrotoI part if(th_wtoI_id != -1){ - F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); - F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); - F.block(p_id, th_wtoI_id, 3, 3) = - R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); - F.block(v_id, th_wtoI_id, 3, 3) = - R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); + F.block<3,3>(th_wtoI_id, th_wtoI_id).setIdentity(); + F.block<3,3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); + F.block<3,3>(p_id, th_wtoI_id) = - R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); + F.block<3,3>(v_id, th_wtoI_id) = - R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); } - - G.block(th_id, 0, 3, 3) = -Jr * dt *R_wtoI * Dw; - G.block(p_id, 0, 3, 3) = R_k.transpose() * Xi_4 *R_wtoI * Dw; - G.block(v_id, 0, 3, 3) = R_k.transpose() * Xi_3 *R_wtoI * Dw; - //G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-log_so3(dR)) * dt; - G.block(th_id,3,3,3) = Jr*dt*R_wtoI*Dw*Tg*R_atoI*Da; - G.block(p_id, 3, 3, 3) = -R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) * R_atoI * Da; - G.block(v_id, 3, 3, 3) = -R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) * R_atoI * Da; - G.block(bg_id, 6, 3, 3) = dt*Eigen::Matrix::Identity(); - G.block(ba_id, 9, 3, 3) = dt*Eigen::Matrix::Identity(); + // construct the G part + G.block<3,3>(th_id, 0) = -Jr * dt *R_wtoI * Dw; + G.block<3,3>(p_id, 0) = R_k.transpose() * Xi_4 *R_wtoI * Dw; + G.block<3,3>(v_id, 0) = R_k.transpose() * Xi_3 *R_wtoI * Dw; + G.block<3,3>(th_id,3) = Jr*dt*R_wtoI*Dw*Tg*R_atoI*Da; + G.block<3,3>(p_id, 3) = -R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) * R_atoI * Da; + G.block<3,3>(v_id, 3) = -R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) * R_atoI * Da; + G.block<3,3>(bg_id, 6) = dt*Eigen::Matrix::Identity(); + G.block<3,3>(ba_id, 9) = dt*Eigen::Matrix::Identity(); } @@ -707,8 +663,6 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - - Eigen::Matrix Da = state->Da(); Eigen::Matrix Dw = state->Dw(); Eigen::Matrix Tg = state->Tg(); @@ -719,33 +673,33 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d Eigen::Matrix Jr = Jr_so3(w_k * dt); // for theta - F.block(th_id, th_id, 3, 3) = dR_ktok1; + F.block<3,3>(th_id, th_id) = dR_ktok1; //F.block(th_id, bg_id, 3, 3).noalias() = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; - F.block(th_id, bg_id, 3, 3).noalias() = -dR_ktok1 * Jr_so3(-w_hat * dt) * dt * R_wtoI * Dw; - F.block(th_id, ba_id, 3, 3).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block<3,3>(th_id, bg_id).noalias() = - Jr * dt * R_wtoI * Dw; + F.block<3,3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; // for position - F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); - F.block(p_id, p_id, 3, 3).setIdentity(); - F.block(p_id, v_id, 3, 3) = Eigen::Matrix::Identity() * dt; - F.block(p_id, ba_id, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + F.block<3,3>(p_id, th_id).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); + F.block<3,3>(p_id, p_id).setIdentity(); + F.block<3,3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; + F.block<3,3>(p_id, ba_id) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; // for velocity - F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); - F.block(v_id, v_id, 3, 3).setIdentity(); - F.block(v_id, ba_id, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; + F.block<3,3>(v_id, th_id).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); + F.block<3,3>(v_id, v_id).setIdentity(); + F.block<3,3>(v_id, ba_id) = -R_k.transpose() * dt * R_atoI * Da; // for bg - F.block(bg_id, bg_id, 3, 3).setIdentity(); + F.block<3,3>(bg_id, bg_id).setIdentity(); // for ba - F.block(ba_id, ba_id, 3, 3).setIdentity(); + F.block<3,3>(ba_id, ba_id).setIdentity(); // begin to add the state transition matrix for the omega intrinsics part if(Dw_id != -1) { F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); - F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr_so3(w_hat * dt) * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); + F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); } // begin to add the state transition matrix for the acc intrinsics part @@ -758,16 +712,16 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the acc intrinsics part if(th_atoI_id != -1) { - F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); - F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); - F.block(p_id, th_atoI_id, 3, 3) = 0.5 * R_k.transpose() * dt * dt *ov_core::skew_x(a_k); - F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * dt *ov_core::skew_x(a_k); + F.block<3,3>(th_atoI_id, th_atoI_id).setIdentity(); + F.block<3,3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block<3,3>(p_id, th_atoI_id) = 0.5 * R_k.transpose() * dt * dt *ov_core::skew_x(a_k); + F.block<3,3>(v_id, th_atoI_id) = R_k.transpose() * dt *ov_core::skew_x(a_k); } // begin to add the state transition matrix for the gyro intrinsics part if(th_wtoI_id != -1) { - F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); - F.block(th_id, th_wtoI_id, 3, 3) = Jr_so3(w_hat * dt) * dt * ov_core::skew_x(w_k); + F.block<3,3>(th_wtoI_id, th_wtoI_id).setIdentity(); + F.block<3,3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); } // begin to add the state transition matrix for the gravity sensitivity part @@ -777,12 +731,12 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d } // Noise jacobian - G.block(th_id, 0, 3, 3) = -Jr_so3(-w_hat * dt) * dt * R_wtoI * Dw; - G.block(th_id, 3, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - G.block(v_id, 3, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; - G.block(p_id, 3, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; - G.block(bg_id, 6, 3, 3) = dt*Eigen::Matrix::Identity(); - G.block(ba_id, 9, 3, 3) = dt*Eigen::Matrix::Identity(); + G.block<3,3>(th_id, 0) = -Jr * dt * R_wtoI * Dw; + G.block<3,3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block<3,3>(v_id, 3) = -R_k.transpose() * dt * R_atoI * Da; + G.block<3,3>(p_id, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + G.block<3,3>(bg_id, 6) = dt*Eigen::Matrix::Identity(); + G.block<3,3>(ba_id, 9) = dt*Eigen::Matrix::Identity(); } @@ -811,7 +765,7 @@ void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, // Orientation: Equation (101) and (103) and of Trawny indirect TR Eigen::Matrix bigO; - if (w_norm > 1e-20) { + if (w_norm > 1e-12) { bigO = cos(0.5 * w_norm * dt) * I_4x4 + 1 / w_norm * sin(0.5 * w_norm * dt) * Omega(w_hat); } else { bigO = I_4x4 + 0.5 * dt * Omega(w_hat); @@ -1106,10 +1060,9 @@ Eigen::MatrixXd Propagator::compute_H_Tg(std::shared_ptr state, const Eig double a_3 = a_inI(2); // intrinsic parameters - Eigen::MatrixXd H_Tg; + Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3,9); if(state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity){ - H_Tg = Eigen::MatrixXd::Zero(3,9); H_Tg << a_1*I_3x3, a_2*I_3x3, a_3*I_3x3; } diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index 373635d85..d70e22994 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -61,20 +61,18 @@ State::State(StateOptions &options) { } if(options.imu_model == 0){ - // kalibr model + // if kalibr model, R_GyrotoI is calibrated _imu_quat_gyrotoI->set_local_id(current_id); _variables.push_back(_imu_quat_gyrotoI); current_id += _imu_quat_gyrotoI->size(); }else{ - // rpng model + // if rpng model, R_AcctoI is calibrated _imu_quat_acctoI->set_local_id(current_id); _variables.push_back(_imu_quat_acctoI); current_id += _imu_quat_acctoI->size(); } } - - // Camera to IMU time offset _calib_dt_CAMtoIMU = std::make_shared(1); if (_options.do_calib_camera_timeoffset) { @@ -122,8 +120,10 @@ State::State(StateOptions &options) { _Cov.block<9,9>(_imu_x_tg->id(),_imu_x_tg->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); } if(_options.imu_model == 0){ + // if kalibr model, R_GyrotoI is calibrated _Cov.block<3,3>(_imu_quat_gyrotoI->id(), _imu_quat_gyrotoI->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); }else{ + // if rpng model, R_AcctoI is calibrated _Cov.block<3,3>(_imu_quat_acctoI->id(),_imu_quat_acctoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } } @@ -152,10 +152,12 @@ State::State(StateOptions &options) { Eigen::Matrix3d State::Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); if(_options.imu_model == 0){ + // if kalibr model, lower triangular of the matrix is used Dw << _imu_x_dw->value()(0), 0, 0, _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), _imu_x_dw->value()(5); }else{ + // if rpng model, upper triangular of the matrix is used Dw << _imu_x_dw->value()(0), _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), 0, 0, _imu_x_dw->value()(5); @@ -167,10 +169,12 @@ Eigen::Matrix3d State::Dw() { Eigen::Matrix3d State::Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); if(_options.imu_model == 0){ + // if kalibr model, lower triangular of the matrix is used Da << _imu_x_da->value()(0), 0, 0, _imu_x_da->value()(1), _imu_x_da->value()(3), 0, _imu_x_da->value()(2), _imu_x_da->value()(4), _imu_x_da->value()(5); }else{ + // if rpng model, upper triangular of the matrix is used Da << _imu_x_da->value()(0), _imu_x_da->value()(1), _imu_x_da->value()(3), 0, _imu_x_da->value()(2), _imu_x_da->value()(4), 0, 0, _imu_x_da->value()(5); From 2f4a1a30a04fe19ff451a271a60fad097b49ff6a Mon Sep 17 00:00:00 2001 From: yangyulin Date: Mon, 20 Dec 2021 11:23:51 -0800 Subject: [PATCH 16/60] add more comments for the intergration functions --- ov_msckf/src/state/Propagator.h | 716 ++++++++++++++++++-------------- 1 file changed, 406 insertions(+), 310 deletions(-) diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 737af113d..225ae3166 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -36,316 +36,412 @@ namespace ov_msckf { * We then compute the state transition matrix at each step and update the state and covariance. * For derivations look at @ref propagation page which has detailed equations. */ -class Propagator { - -public: - - /** - * @brief Default constructor - * @param noises imu noise characteristics (continuous time) - * @param gravity_mag Global gravity magnitude of the system (normally 9.81) - */ - Propagator( ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { - _imu_config.sigma_w_2 = std::pow(_imu_config.sigma_w, 2); - _imu_config.sigma_a_2 = std::pow(_imu_config.sigma_a, 2); - _imu_config.sigma_wb_2 = std::pow(_imu_config.sigma_wb, 2); - _imu_config.sigma_ab_2 = std::pow(_imu_config.sigma_ab, 2); - last_prop_time_offset = 0.0; - _gravity << 0.0, 0.0, gravity_mag; - } - - /** - * @brief Stores incoming inertial readings - * @param message Contains our timestamp and inertial information - */ - void feed_imu(const ov_core::ImuData &message) { - - // Append it to our vector - imu_data.emplace_back(message); - - // Loop through and delete imu messages that are older then 10 seconds - // TODO: we should probably have more elegant logic then this - // TODO: but this prevents unbounded memory growth and slow prop with high freq imu - auto it0 = imu_data.begin(); - while (it0 != imu_data.end()) { - if (message.timestamp - (*it0).timestamp > 10) { - it0 = imu_data.erase(it0); - } else { - it0++; - } - } - } - - /** - * @brief Propagate state up to given timestamp and then clone - * - * This will first collect all imu readings that occured between the - * *current* state time and the new time we want the state to be at. - * If we don't have any imu readings we will try to extrapolate into the future. - * After propagating the mean and covariance using our dynamics, - * We clone the current imu pose as a new clone in our state. - * - * @param state Pointer to state - * @param timestamp Time to propagate to and clone at - */ - void propagate_and_clone(std::shared_ptr state, double timestamp); - - /** - * @brief Gets what the state and its covariance will be at a given timestamp - * - * This can be used to find what the state will be in the "future" without propagating it. - * This will propagate a clone of the current IMU state and its covariance matrix. - * This is typically used to provide high frequency pose estimates between updates. - * - * @param state Pointer to state - * @param timestamp Time to propagate to - * @param state_plus The propagated state (q_GtoI, p_IinG, v_IinG, w_IinI) - */ - void fast_state_propagate(std::shared_ptr state, double timestamp, Eigen::Matrix &state_plus); - - /** - * @brief Helper function that given current imu data, will select imu readings between the two times. - * - * This will create measurements that we will integrate with, and an extra measurement at the end. - * We use the @ref interpolate_data() function to "cut" the imu readings at the begining and end of the integration. - * The timestamps passed should already take into account the time offset values. - * - * @param imu_data IMU data we will select measurements from - * @param time0 Start timestamp - * @param time1 End timestamp - * @param warn If we should warn if we don't have enough IMU to propagate with (e.g. fast prop will get warnings otherwise) - * @return Vector of measurements (if we could compute them) - */ - static std::vector select_imu_readings(const std::vector &imu_data, double time0, double time1, - bool warn = true); - - /** - * @brief Nice helper function that will linearly interpolate between two imu messages. - * - * This should be used instead of just "cutting" imu messages that bound the camera times - * Give better time offset if we use this function, could try other orders/splines if the imu is slow. - * - * @param imu_1 imu at begining of interpolation interval - * @param imu_2 imu at end of interpolation interval - * @param timestamp Timestamp being interpolated to - */ - static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) { - // time-distance lambda - double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp); - // PRINT_DEBUG("lambda - %d\n", lambda); - // interpolate between the two times - ov_core::ImuData data; - data.timestamp = timestamp; - data.am = (1 - lambda) * imu_1.am + lambda * imu_2.am; - data.wm = (1 - lambda) * imu_1.wm + lambda * imu_2.wm; - return data; - } - -protected: - /// Estimate for time offset at last propagation time - double last_prop_time_offset = 0.0; - bool have_last_prop_time_offset = false; - - /** - * @brief Propagates the state forward using the imu data and computes the noise covariance and state-transition - * matrix of this interval. - * - * This function can be replaced with analytical/numerical integration or when using a different state representation. - * This contains our state transition matrix along with how our noise evolves in time. - * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref error_prop page for details on how this was derived. - * - * @param state Pointer to state - * @param data_minus imu readings at beginning of interval - * @param data_plus imu readings at end of interval - * @param F State-transition matrix over the interval - * @param Qd Discrete-time noise covariance over the interval - */ - void predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, - Eigen::MatrixXd &F, Eigen::MatrixXd &Qd); - - /** - * @brief Discrete imu mean propagation. - * - * See @ref propagation for details on these equations. - * \f{align*}{ - * \text{}^{I_{k+1}}_{G}\hat{\bar{q}} - * &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) - * \text{}^{I_{k}}_{G}\hat{\bar{q}} \\ - * ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t - * +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ - * ^G\hat{\mathbf{p}}_{I_{k+1}} - * &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t - * - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 - * + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 - * \f} - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat1 Angular velocity with bias removed - * @param a_hat1 Linear acceleration with bias removed - * @param w_hat2 Next angular velocity with bias removed - * @param a_hat2 Next linear acceleration with bias removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - */ - void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, - const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, - Eigen::Vector3d &new_p); - - /** - * @brief RK4 imu mean propagation. - * - * See this wikipedia page on [Runge-Kutta Methods](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods). - * We are doing a RK4 method, [this wolframe page](http://mathworld.wolfram.com/Runge-KuttaMethod.html) has the forth order equation - * defined below. We define function \f$ f(t,y) \f$ where y is a function of time t, see @ref imu_kinematic for the definition of the - * continous-time functions. - * - * \f{align*}{ - * {k_1} &= f({t_0}, y_0) \Delta t \\ - * {k_2} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_1} ) \Delta t \\ - * {k_3} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_2} ) \Delta t \\ - * {k_4} &= f( {t_0} + {\Delta t}, y_0 + {k_3} ) \Delta t \\ - * y_{0+\Delta t} &= y_0 + \left( {{1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4}} \right) - * \f} - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat1 Angular velocity with bias removed - * @param a_hat1 Linear acceleration with bias removed - * @param w_hat2 Next angular velocity with bias removed - * @param a_hat2 Next linear acceleration with bias removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - */ - void predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, - const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, - Eigen::Vector3d &new_p); - - - /** - * @brief Analytically compute the integration componenets based on ACI^2 - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param Xi_sum All the needed integration componenets, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 - */ - void compute_Xi_sum(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Matrix &Xi_sum); - - - /** - * @brief Analytically predict IMU mean - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - * @param Xi_sum All the needed integration componenets, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 - */ - void predict_mean_analytic(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, - Eigen::Matrix &Xi_sum); - - - /** - * @brief Analytically compute state transition matrix F and noise Jacobian G - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - * @param Xi_sum All the needed integration componenets, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 - * @param F State transition matrix - * @param G Noise Jacobian - */ - void compute_F_and_G_analytic(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, - const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, - const Eigen::Matrix &Xi_sum, - Eigen::MatrixXd &F, Eigen::MatrixXd &G); - - /** - * @brief compute state transition matrix F and noise Jacobian G - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - * @param F State transition matrix - * @param G Noise Jacobian - */ - void compute_F_and_G_discrete(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, - const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, - Eigen::MatrixXd &F, Eigen::MatrixXd &G); - - /** - * @brief compute the Jacobians for Dw - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed - */ - Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); - - /** - * @brief compute the Jacobians for Da - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param a_uncorrected Linear acceleration in gyro frame with bias removed - */ - Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); - - /** - * @brief compute the Jacobians for Tg - * - * See the paper for ACI^2 or IJRR paper - * - * @param state Pointer to state - * @param a_inI Linear acceleration with bias removed - */ - Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); - - /// Container for the noise values - ov_core::ImuConfig _imu_config; - - /// Our history of IMU messages (time, angular, linear) - std::vector imu_data; - - /// Gravity vector - Eigen::Vector3d _gravity; -}; + class Propagator { + + public: + + /** + * @brief Default constructor + * @param noises imu noise characteristics (continuous time) + * @param gravity_mag Global gravity magnitude of the system (normally 9.81) + */ + Propagator( ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { + _imu_config.sigma_w_2 = std::pow(_imu_config.sigma_w, 2); + _imu_config.sigma_a_2 = std::pow(_imu_config.sigma_a, 2); + _imu_config.sigma_wb_2 = std::pow(_imu_config.sigma_wb, 2); + _imu_config.sigma_ab_2 = std::pow(_imu_config.sigma_ab, 2); + last_prop_time_offset = 0.0; + _gravity << 0.0, 0.0, gravity_mag; + } + + /** + * @brief Stores incoming inertial readings + * @param message Contains our timestamp and inertial information + */ + void feed_imu(const ov_core::ImuData &message) { + + // Append it to our vector + imu_data.emplace_back(message); + + // Loop through and delete imu messages that are older then 10 seconds + // TODO: we should probably have more elegant logic then this + // TODO: but this prevents unbounded memory growth and slow prop with high freq imu + auto it0 = imu_data.begin(); + while (it0 != imu_data.end()) { + if (message.timestamp - (*it0).timestamp > 10) { + it0 = imu_data.erase(it0); + } else { + it0++; + } + } + } + + /** + * @brief Propagate state up to given timestamp and then clone + * + * This will first collect all imu readings that occured between the + * *current* state time and the new time we want the state to be at. + * If we don't have any imu readings we will try to extrapolate into the future. + * After propagating the mean and covariance using our dynamics, + * We clone the current imu pose as a new clone in our state. + * + * @param state Pointer to state + * @param timestamp Time to propagate to and clone at + */ + void propagate_and_clone(std::shared_ptr state, double timestamp); + + /** + * @brief Gets what the state and its covariance will be at a given timestamp + * + * This can be used to find what the state will be in the "future" without propagating it. + * This will propagate a clone of the current IMU state and its covariance matrix. + * This is typically used to provide high frequency pose estimates between updates. + * + * @param state Pointer to state + * @param timestamp Time to propagate to + * @param state_plus The propagated state (q_GtoI, p_IinG, v_IinG, w_IinI) + */ + void fast_state_propagate(std::shared_ptr state, double timestamp, Eigen::Matrix &state_plus); + + /** + * @brief Helper function that given current imu data, will select imu readings between the two times. + * + * This will create measurements that we will integrate with, and an extra measurement at the end. + * We use the @ref interpolate_data() function to "cut" the imu readings at the begining and end of the integration. + * The timestamps passed should already take into account the time offset values. + * + * @param imu_data IMU data we will select measurements from + * @param time0 Start timestamp + * @param time1 End timestamp + * @param warn If we should warn if we don't have enough IMU to propagate with (e.g. fast prop will get warnings otherwise) + * @return Vector of measurements (if we could compute them) + */ + static std::vector select_imu_readings(const std::vector &imu_data, double time0, double time1, + bool warn = true); + + /** + * @brief Nice helper function that will linearly interpolate between two imu messages. + * + * This should be used instead of just "cutting" imu messages that bound the camera times + * Give better time offset if we use this function, could try other orders/splines if the imu is slow. + * + * @param imu_1 imu at begining of interpolation interval + * @param imu_2 imu at end of interpolation interval + * @param timestamp Timestamp being interpolated to + */ + static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) { + // time-distance lambda + double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp); + // PRINT_DEBUG("lambda - %d\n", lambda); + // interpolate between the two times + ov_core::ImuData data; + data.timestamp = timestamp; + data.am = (1 - lambda) * imu_1.am + lambda * imu_2.am; + data.wm = (1 - lambda) * imu_1.wm + lambda * imu_2.wm; + return data; + } + + protected: + /// Estimate for time offset at last propagation time + double last_prop_time_offset = 0.0; + bool have_last_prop_time_offset = false; + + /** + * @brief Propagates the state forward using the imu data and computes the noise covariance and state-transition + * matrix of this interval. + * + * This function can be replaced with analytical/numerical integration or when using a different state representation. + * This contains our state transition matrix along with how our noise evolves in time. + * If you have other state variables besides the IMU that evolve you would add them here. + * See the @ref error_prop page for details on how discrete model was derived. + * See the @ref imu_intrinsic page for details on how analytic model was derived. + * + * @param state Pointer to state + * @param data_minus imu readings at beginning of interval + * @param data_plus imu readings at end of interval + * @param F State-transition matrix over the interval + * @param Qd Discrete-time noise covariance over the interval + */ + void predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, + Eigen::MatrixXd &F, Eigen::MatrixXd &Qd); + + /** + * @brief Discrete imu mean propagation. + * + * See @ref propagation for details on these equations. + * \f{align*}{ + * \text{}^{I_{k+1}}_{G}\hat{\bar{q}} + * &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) + * \text{}^{I_{k}}_{G}\hat{\bar{q}} \\ + * ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t + * +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ + * ^G\hat{\mathbf{p}}_{I_{k+1}} + * &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t + * - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + * + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat1 Angular velocity with bias removed + * @param a_hat1 Linear acceleration with bias removed + * @param w_hat2 Next angular velocity with bias removed + * @param a_hat2 Next linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + */ + void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, + const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, + Eigen::Vector3d &new_p); + + /** + * @brief RK4 imu mean propagation. + * + * See this wikipedia page on [Runge-Kutta Methods](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods). + * We are doing a RK4 method, [this wolframe page](http://mathworld.wolfram.com/Runge-KuttaMethod.html) has the forth order equation + * defined below. We define function \f$ f(t,y) \f$ where y is a function of time t, see @ref imu_kinematic for the definition of the + * continous-time functions. + * + * \f{align*}{ + * {k_1} &= f({t_0}, y_0) \Delta t \\ + * {k_2} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_1} ) \Delta t \\ + * {k_3} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_2} ) \Delta t \\ + * {k_4} &= f( {t_0} + {\Delta t}, y_0 + {k_3} ) \Delta t \\ + * y_{0+\Delta t} &= y_0 + \left( {{1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4}} \right) + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat1 Angular velocity with bias removed + * @param a_hat1 Linear acceleration with bias removed + * @param w_hat2 Next angular velocity with bias removed + * @param a_hat2 Next linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + */ + void predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, + const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, + Eigen::Vector3d &new_p); + + + /** + * @brief Analytically compute the integration components based on ACI^2 + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) + * + * For computing Xi_1, Xi_2, Xi_3 and Xi_4, you can refer to @ref imu_intrinsics or the following equations: + * \f{align*}{ + * \boldsymbol{\Xi}_1 & = \mathbf{I}_3 \delta t + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} \lfloor \hat{\mathbf{k}} \rfloor + * + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 + * \boldsymbol{\Xi}_2 & = \frac{1}{2} \delta t^2 \mathbf{I}_3 + + * \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor + * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 + * \boldsymbol{\Xi}_3 &= + * \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor + * + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + * + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor ^2 + * + \left( + * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \right) + * \lfloor \hat{\mathbf{k}} \rfloor ^2 \lfloor\hat{\mathbf{a}} \rfloor + * + \left( + * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + * - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 + * \boldsymbol{\Xi}_4 + * & = \frac{1}{6}\delta t^3 \lfloor\hat{\mathbf{a}} \rfloor + * + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} + * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + * + \left( + * \frac{2(1- \cos(\hat{\omega} \delta t)) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t)}{\hat{\omega}^3} + * \right) + * \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + * + \left( + * \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t}{\hat{\omega}^3} + + * \frac{\delta t^3}{6} + * \right) + * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + * + + * \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} + * \lfloor \hat{\mathbf{k}} \rfloor^2\lfloor\hat{\mathbf{a}} \rfloor + * + + * \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} + * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + * + + * \frac{4 \cos(\hat{\omega} \delta t) - 4 + (\hat{\omega} \delta t)^2 + \hat{\omega} \delta t \sin(\hat{\omega} \delta t) } + * {\hat{\omega}^3} + * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 + * \f} + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 in order + */ + void compute_Xi_sum(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Matrix &Xi_sum); + + + /** + * @brief Analytically predict IMU mean based on ACI^2 + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) + * + * \f{align*}{ + * {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}^{\top}_k {}^{I_k}_G\hat{\mathbf{R}} \\ + * {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\delta t_k + {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{p}}_k - \frac{1}{2}{}^G\mathbf{g}\delta t^2_k \\ + * {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{v}}_k - {}^G\mathbf{g}\delta t_k + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + */ + void predict_mean_analytic(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, + Eigen::Matrix &Xi_sum); + + + /** + * @brief Analytically compute state transition matrix F and noise Jacobian G based on ACI^2 + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) + * + * This function is for analytical integration or when using a different state representation. + * This contains our state transition matrix and noise Jacobians. + * If you have other state variables besides the IMU that evolve you would add them here. + * See the @ref imu_intrinsics page for details on how this was derived. + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + * @param F State transition matrix + * @param G Noise Jacobian + */ + void compute_F_and_G_analytic(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, + const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, + const Eigen::Matrix &Xi_sum, + Eigen::MatrixXd &F, Eigen::MatrixXd &G); + + /** + * @brief compute state transition matrix F and noise Jacobian G + * + * This function is for analytical integration or when using a different state representation. + * This contains our state transition matrix and noise Jacobians. + * If you have other state variables besides the IMU that evolve you would add them here. + * See the @ref imu_intrinsics page for details on how this was derived. + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param F State transition matrix + * @param G Noise Jacobian + */ + void compute_F_and_G_discrete(std::shared_ptr state, double dt, + const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, + const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, + Eigen::MatrixXd &F, Eigen::MatrixXd &G); + + /** + * @brief compute the Jacobians for Dw + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * For kalibr model + * \f{align*}{ + * \mathbf{H}_{Dw} & = + * \begin{bmatrix} + * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 + * \end{bmatrix} + * \f} + * For rpng model: + * \f{align*}{ + * \mathbf{H}_{Dw} & = + * \begin{bmatrix} + * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * @param state Pointer to state + * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed + */ + Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); + + /** + * @brief compute the Jacobians for Da + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * + * For kalibr model + * \f{align*}{ + * \mathbf{H}_{Da} & = + * \begin{bmatrix} + * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * For rpng: + * \f{align*}{ + * \mathbf{H}_{Da} & = + * \begin{bmatrix} + * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 + * \end{bmatrix} + * \f} + * @param state Pointer to state + * @param a_uncorrected Linear acceleration in gyro frame with bias removed + */ + Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); + + /** + * @brief compute the Jacobians for Tg + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * + * + * \f{align*}{ + * \mathbf{H}_{Tg} & = + * \begin{bmatrix} + * {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * @param state Pointer to state + * @param a_inI Linear acceleration with bias removed + */ + Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); + + /// Container for the noise values + ov_core::ImuConfig _imu_config; + + /// Our history of IMU messages (time, angular, linear) + std::vector imu_data; + + /// Gravity vector + Eigen::Vector3d _gravity; + }; } // namespace ov_msckf From 63aebb7aed3b8e233a2fdb962a90cacbb4823d8b Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 11:49:21 -0500 Subject: [PATCH 17/60] small cleanups, add default identities to state init --- config/euroc_mav/estimator_config.yaml | 2 +- config/euroc_mav/kalibr_imu_chain.yaml | 2 +- config/kaist/kalibr_imu_chain.yaml | 4 +- config/kaist_vio/kalibr_imu_chain.yaml | 32 +- config/rpng_aruco/kalibr_imu_chain.yaml | 4 +- config/rpng_ironsides/kalibr_imu_chain.yaml | 4 +- config/rpng_sim/kalibr_imu_chain.yaml | 32 +- config/tum_vi/kalibr_imu_chain.yaml | 4 +- config/uzhfpv_indoor/kalibr_imu_chain.yaml | 4 +- config/uzhfpv_indoor_45/kalibr_imu_chain.yaml | 4 +- ov_core/src/utils/sensor_data.h | 85 +-- ov_eval/src/calc/ResultSimulation.cpp | 410 +++++++------- ov_eval/src/calc/ResultSimulation.h | 5 + ov_msckf/src/core/VioManager.cpp | 4 +- ov_msckf/src/core/VioManagerOptions.h | 28 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 14 +- ov_msckf/src/ros/RosVisualizerHelper.h | 130 ++--- ov_msckf/src/sim/Simulator.cpp | 20 +- ov_msckf/src/state/Propagator.cpp | 534 +++++++++--------- ov_msckf/src/state/State.cpp | 99 ++-- ov_msckf/src/state/StateOptions.h | 13 +- 21 files changed, 664 insertions(+), 770 deletions(-) diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index ae2b7540a..70bf2cf2a 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -13,7 +13,7 @@ calib_cam_extrinsics: true # if the transform between camera and IMU should be o calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized calib_imu_intrinsics: false # if imu intrinsics should be calibrated -calib_imu_g_sensitivity: false # if gravity sensitivity should be calibrated +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 50 # number of features in our state vector diff --git a/config/euroc_mav/kalibr_imu_chain.yaml b/config/euroc_mav/kalibr_imu_chain.yaml index 7825b7afe..09f751ae5 100644 --- a/config/euroc_mav/kalibr_imu_chain.yaml +++ b/config/euroc_mav/kalibr_imu_chain.yaml @@ -10,10 +10,10 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: kalibr # 0: kalibr and 1: rpng rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/kaist/kalibr_imu_chain.yaml b/config/kaist/kalibr_imu_chain.yaml index 6bd3c3213..572575357 100644 --- a/config/kaist/kalibr_imu_chain.yaml +++ b/config/kaist/kalibr_imu_chain.yaml @@ -20,6 +20,7 @@ imu0: rostopic: /imu/data_raw time_offset: 0.0 update_rate: 500.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] @@ -39,5 +40,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/kaist_vio/kalibr_imu_chain.yaml b/config/kaist_vio/kalibr_imu_chain.yaml index 0d33e793d..ecc3f0b23 100644 --- a/config/kaist_vio/kalibr_imu_chain.yaml +++ b/config/kaist_vio/kalibr_imu_chain.yaml @@ -13,24 +13,24 @@ imu0: rostopic: /mavros/imu/data time_offset: 0.0 update_rate: 100.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] R_ItoGyro: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] Ta: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] R_ItoAcc: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] Tg: - - [0.0, 0.0, 0.0] - - [0.0, 0.0, 0.0] - - [0.0, 0.0, 0.0] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/rpng_aruco/kalibr_imu_chain.yaml b/config/rpng_aruco/kalibr_imu_chain.yaml index 29721bd37..3d881e69f 100644 --- a/config/rpng_aruco/kalibr_imu_chain.yaml +++ b/config/rpng_aruco/kalibr_imu_chain.yaml @@ -13,6 +13,7 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] @@ -32,5 +33,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/rpng_ironsides/kalibr_imu_chain.yaml b/config/rpng_ironsides/kalibr_imu_chain.yaml index fee670bc0..0ab0f2dfc 100644 --- a/config/rpng_ironsides/kalibr_imu_chain.yaml +++ b/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -13,6 +13,7 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] @@ -32,5 +33,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 45ce13f9f..715a5a4ad 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -13,24 +13,24 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 + model: "calibrated" # calibrated (same as kalibr), kalibr, rpng Tw: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] R_ItoGyro: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] Ta: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] R_ItoAcc: - - [1.0, 0.0, 0.0] - - [0.0, 1.0, 0.0] - - [0.0, 0.0, 1.0] + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] Tg: - - [0.0, 0.0, 0.0] - - [0.0, 0.0, 0.0] - - [0.0, 0.0, 0.0] - model: rpng # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/tum_vi/kalibr_imu_chain.yaml b/config/tum_vi/kalibr_imu_chain.yaml index b7dfeadcd..349f66b9a 100644 --- a/config/tum_vi/kalibr_imu_chain.yaml +++ b/config/tum_vi/kalibr_imu_chain.yaml @@ -13,6 +13,7 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] @@ -32,5 +33,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/config/uzhfpv_indoor/kalibr_imu_chain.yaml index 3dd4b198f..acbd81742 100644 --- a/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -17,6 +17,7 @@ imu0: rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] @@ -36,5 +37,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml index a192a592c..7125c7678 100644 --- a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -17,6 +17,7 @@ imu0: rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] @@ -36,5 +37,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] - model: kalibr # 0: kalibr and 1: rpng \ No newline at end of file + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index 677ca0282..31e9a732c 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -25,6 +25,7 @@ #include #include #include + #include "utils/quat_ops.h" namespace ov_core { @@ -78,19 +79,19 @@ struct CameraData { }; /** - * @brief Struct of our imu noise parameters - */ + * @brief Struct of our imu noise parameters + */ struct ImuConfig { /// imu model, 0: Kalibr model and 1: RPNG model int imu_model = 0; /// imu intrinsics - Eigen::Matrix imu_x_dw; /// the columnwise elements for Dw - Eigen::Matrix imu_x_da; /// the columnwise elements for Da - Eigen::Matrix imu_x_tg; /// the ccolumnwise elements for Tg - Eigen::Matrix imu_quat_AcctoI; /// the JPL quat for R_AcctoI - Eigen::Matrix imu_quat_GyrotoI; /// the JPL quat for R_GyrotoI + Eigen::Matrix imu_x_dw; /// the columnwise elements for Dw + Eigen::Matrix imu_x_da; /// the columnwise elements for Da + Eigen::Matrix imu_x_tg; /// the ccolumnwise elements for Tg + Eigen::Matrix imu_quat_AcctoI; /// the JPL quat for R_AcctoI + Eigen::Matrix imu_quat_GyrotoI; /// the JPL quat for R_GyrotoI /// Gyroscope white noise (rad/s/sqrt(hz)) double sigma_w = 1.6968e-04; @@ -125,90 +126,56 @@ struct ImuConfig { } /// get the IMU Dw - Eigen::Matrix3d Dw(){ + Eigen::Matrix3d Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if(imu_model == 0){ + if (imu_model == 0) { // Kalibr model with lower triangular of the matrix - Dw << - imu_x_dw(0), 0, 0, - imu_x_dw(1), imu_x_dw(3), 0, - imu_x_dw(2), imu_x_dw(4), imu_x_dw(5); - }else{ + Dw << imu_x_dw(0), 0, 0, imu_x_dw(1), imu_x_dw(3), 0, imu_x_dw(2), imu_x_dw(4), imu_x_dw(5); + } else { // rpng model with upper triangular of the matrix - Dw << - imu_x_dw(0), imu_x_dw(1), imu_x_dw(3), - 0, imu_x_dw(2), imu_x_dw(4), - 0, 0, imu_x_dw(5); + Dw << imu_x_dw(0), imu_x_dw(1), imu_x_dw(3), 0, imu_x_dw(2), imu_x_dw(4), 0, 0, imu_x_dw(5); } return Dw; } /// get the IMU Da - Eigen::Matrix3d Da(){ + Eigen::Matrix3d Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if(imu_model == 0){ + if (imu_model == 0) { // kalibr model with lower triangular of the matrix - Da << - imu_x_da(0), 0, 0, - imu_x_da(1), imu_x_da(3), 0, - imu_x_da(2), imu_x_da(4), imu_x_da(5); - }else{ + Da << imu_x_da(0), 0, 0, imu_x_da(1), imu_x_da(3), 0, imu_x_da(2), imu_x_da(4), imu_x_da(5); + } else { // rpng model with upper triangular of the matrix - Da << - imu_x_da(0), imu_x_da(1), imu_x_da(3), - 0, imu_x_da(2), imu_x_da(4), - 0, 0, imu_x_da(5); + Da << imu_x_da(0), imu_x_da(1), imu_x_da(3), 0, imu_x_da(2), imu_x_da(4), 0, 0, imu_x_da(5); } return Da; } /// get the IMU Tw - Eigen::Matrix3d Tw() { - return Dw().inverse(); - } + Eigen::Matrix3d Tw() { return Dw().inverse(); } /// get the IMU Ta - Eigen::Matrix3d Ta() { - return Da().inverse(); - } + Eigen::Matrix3d Ta() { return Da().inverse(); } /// get the IMU Tg - Eigen::Matrix3d Tg(){ + Eigen::Matrix3d Tg() { Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << imu_x_tg(0), imu_x_tg(3), imu_x_tg(6), - imu_x_tg(1), imu_x_tg(4), imu_x_tg(7), - imu_x_tg(2), imu_x_tg(5), imu_x_tg(8); + Tg << imu_x_tg(0), imu_x_tg(3), imu_x_tg(6), imu_x_tg(1), imu_x_tg(4), imu_x_tg(7), imu_x_tg(2), imu_x_tg(5), imu_x_tg(8); return Tg; } /// get the R_AcctoI - Eigen::Matrix3d R_AcctoI(){ - return ov_core::quat_2_Rot(imu_quat_AcctoI); - } + Eigen::Matrix3d R_AcctoI() { return ov_core::quat_2_Rot(imu_quat_AcctoI); } /// get the R_ItoAcc - Eigen::Matrix3d R_ItoAcc(){ - return R_AcctoI().transpose(); - } + Eigen::Matrix3d R_ItoAcc() { return R_AcctoI().transpose(); } /// get the R_GyrotoI - Eigen::Matrix3d R_GyrotoI(){ - return ov_core::quat_2_Rot(imu_quat_GyrotoI); - } + Eigen::Matrix3d R_GyrotoI() { return ov_core::quat_2_Rot(imu_quat_GyrotoI); } /// get the R_ItoGyro - Eigen::Matrix3d R_ItoGyro(){ - return R_GyrotoI().transpose(); - } - - - - + Eigen::Matrix3d R_ItoGyro() { return R_GyrotoI().transpose(); } }; - - - - } // namespace ov_core #endif // OV_CORE_SENSOR_DATA_H \ No newline at end of file diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 7a2361d30..73fe67e36 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -499,261 +499,253 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { #endif } - void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { - // Check that we have cameras - if ((int)est_state.at(0)(18) < 1) { - PRINT_ERROR(YELLOW "You need at least one camera to run estimator and plot results...\n" RESET); - return; - } + // Check that we have cameras + if ((int)est_state.at(0)(18) < 1) { + PRINT_ERROR(YELLOW "You need at least one camera to run estimator and plot results...\n" RESET); + return; + } - // get the parameters id - int num_cam = (int)est_state.at(0)(18); - int imu_model = (int) est_state.at(0)(1 + 16 + 1 + 1 + 15 * num_cam); - int dw_id = 1 + 16 + 1 + 1 + 15 * num_cam + 1; - int dw_cov_id = 1 + 15 + 1 + 1 + 14 * num_cam + 1; - int da_id = dw_id + 6; - int da_cov_id = dw_cov_id + 6; - int tg_id = da_id + 6; - int tg_cov_id = da_cov_id + 6; - int wtoI_id = tg_id + 9; - int wtoI_cov_id = tg_cov_id + 9; - int atoI_id = wtoI_id + 4; - int atoI_cov_id = wtoI_cov_id + 3; + // get the parameters id + int num_cam = (int)est_state.at(0)(18); + int imu_model = (int)est_state.at(0)(1 + 16 + 1 + 1 + 15 * num_cam); + int dw_id = 1 + 16 + 1 + 1 + 15 * num_cam + 1; + int dw_cov_id = 1 + 15 + 1 + 1 + 14 * num_cam + 1; + int da_id = dw_id + 6; + int da_cov_id = dw_cov_id + 6; + int tg_id = da_id + 6; + int tg_cov_id = da_cov_id + 6; + int wtoI_id = tg_id + 9; + int wtoI_cov_id = tg_cov_id + 9; + int atoI_id = wtoI_id + 4; + int atoI_cov_id = wtoI_cov_id + 3; + + // IMU intrinsics statistic storage + std::vector error_dw, error_da, error_tg, error_wtoI, error_atoI; + for (int j = 0; j < 6; j++) { + error_dw.emplace_back(Statistics()); + error_da.emplace_back(Statistics()); + } + for (int j = 0; j < 9; j++) { + error_tg.emplace_back(Statistics()); + } + for (int j = 0; j < 3; j++) { + error_wtoI.emplace_back(Statistics()); + error_atoI.emplace_back(Statistics()); + } + // Loop through and calculate error + double start_time = est_state.at(0)(0); + for (size_t i = 0; i < est_state.size(); i++) { - // IMU intrinsics statistic storage - std::vector error_dw, error_da, error_tg, error_wtoI, error_atoI; + // Exit if we have reached our max time + if ((est_state.at(i)(0) - start_time) > max_time) + break; + // Assert our times are the same + assert(est_state.at(i)(0) == gt_state.at(i)(0)); + + // If we are not calibrating then don't plot it! + if (state_cov.at(i)(dw_cov_id) == 0.0) { + PRINT_WARNING(YELLOW "IMU intrinsics not calibrated online, so will not plot...\n" RESET); + return; + } + + // Loop through IMU parameters and calculate error for (int j = 0; j < 6; j++) { - error_dw.emplace_back(Statistics()); - error_da.emplace_back(Statistics()); + error_dw.at(j).timestamps.push_back(est_state.at(i)(0)); + error_dw.at(j).values.push_back(est_state.at(i)(dw_id + j) - gt_state.at(i)(dw_id + j)); + error_dw.at(j).values_bound.push_back(3 * state_cov.at(i)(dw_cov_id + j)); + error_da.at(j).timestamps.push_back(est_state.at(i)(0)); + error_da.at(j).values.push_back(est_state.at(i)(da_id + j) - gt_state.at(i)(da_id + j)); + error_da.at(j).values_bound.push_back(3 * state_cov.at(i)(da_cov_id + j)); } for (int j = 0; j < 9; j++) { - error_tg.emplace_back(Statistics()); - } - for (int j = 0; j < 3; j++) { - error_wtoI.emplace_back(Statistics()); - error_atoI.emplace_back(Statistics()); + error_tg.at(j).timestamps.push_back(est_state.at(i)(0)); + error_tg.at(j).values.push_back(est_state.at(i)(tg_id + j) - gt_state.at(i)(tg_id + j)); + error_tg.at(j).values_bound.push_back(3 * state_cov.at(i)(tg_cov_id + j)); } - // Loop through and calculate error - double start_time = est_state.at(0)(0); - for (size_t i = 0; i < est_state.size(); i++) { - - // Exit if we have reached our max time - if ((est_state.at(i)(0) - start_time) > max_time) - break; - - // Assert our times are the same - assert(est_state.at(i)(0) == gt_state.at(i)(0)); - - // If we are not calibrating then don't plot it! - if (state_cov.at(i)(dw_cov_id) == 0.0) { - PRINT_WARNING(YELLOW "IMU intrinsics not calibrated online, so will not plot...\n" RESET); - return; - } - - // Loop through IMU parameters and calculate error - for(int j=0; j<6; j++){ - error_dw.at(j).timestamps.push_back(est_state.at(i)(0)); - error_dw.at(j).values.push_back(est_state.at(i)(dw_id + j) - gt_state.at(i)(dw_id + j)); - error_dw.at(j).values_bound.push_back(3 * state_cov.at(i)(dw_cov_id + j)); - error_da.at(j).timestamps.push_back(est_state.at(i)(0)); - error_da.at(j).values.push_back(est_state.at(i)(da_id + j) - gt_state.at(i)(da_id + j)); - error_da.at(j).values_bound.push_back(3 * state_cov.at(i)(da_cov_id + j)); - } - - for(int j=0; j<9; j++){ - error_tg.at(j).timestamps.push_back(est_state.at(i)(0)); - error_tg.at(j).values.push_back(est_state.at(i)(tg_id + j) - gt_state.at(i)(tg_id + j)); - error_tg.at(j).values_bound.push_back(3 * state_cov.at(i)(tg_cov_id + j)); - - } - - - Eigen::Matrix3d e_R_wtoI = ov_core::quat_2_Rot(gt_state.at(i).block<4,1>(wtoI_id, 0)) * - ov_core::quat_2_Rot(est_state.at(i).block<4,1>(wtoI_id, 0)).transpose(); - Eigen::Vector3d ori_wtoI = -180.0 / M_PI * ov_core::log_so3(e_R_wtoI); - - Eigen::Matrix3d e_R_atoI = ov_core::quat_2_Rot(gt_state.at(i).block<4,1>(atoI_id, 0)) * - ov_core::quat_2_Rot(est_state.at(i).block<4,1>(atoI_id, 0)).transpose(); - Eigen::Vector3d ori_atoI = -180.0 / M_PI * ov_core::log_so3(e_R_atoI); - - for(int j=0; j<3; j++){ - error_wtoI.at(j).timestamps.push_back(est_state.at(i)(0)); - error_wtoI.at(j).values.push_back(ori_wtoI(j)); - error_wtoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(wtoI_cov_id + j)); - error_atoI.at(j).timestamps.push_back(est_state.at(i)(0)); - error_atoI.at(j).values.push_back(ori_atoI(j)); - error_atoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(atoI_cov_id + j)); - } - + // Calculate orientation errors + Eigen::Matrix3d e_R_wtoI = ov_core::quat_2_Rot(gt_state.at(i).block<4, 1>(wtoI_id, 0)) * + ov_core::quat_2_Rot(est_state.at(i).block<4, 1>(wtoI_id, 0)).transpose(); + Eigen::Vector3d ori_wtoI = -180.0 / M_PI * ov_core::log_so3(e_R_wtoI); + Eigen::Matrix3d e_R_atoI = ov_core::quat_2_Rot(gt_state.at(i).block<4, 1>(atoI_id, 0)) * + ov_core::quat_2_Rot(est_state.at(i).block<4, 1>(atoI_id, 0)).transpose(); + Eigen::Vector3d ori_atoI = -180.0 / M_PI * ov_core::log_so3(e_R_atoI); + for (int j = 0; j < 3; j++) { + error_wtoI.at(j).timestamps.push_back(est_state.at(i)(0)); + error_wtoI.at(j).values.push_back(ori_wtoI(j)); + error_wtoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(wtoI_cov_id + j)); + error_atoI.at(j).timestamps.push_back(est_state.at(i)(0)); + error_atoI.at(j).values.push_back(ori_atoI(j)); + error_atoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(atoI_cov_id + j)); } + } - - // return if we don't want to plot - if (!doplotting) - return; + // return if we don't want to plot + if (!doplotting) + return; #ifndef HAVE_PYTHONLIBS - PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); + PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; #else - // Plot line colors - std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; + // Plot line colors + std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; - //===================================================== - // Plot this figure - matplotlibcpp::figure_size(800, 500); - std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(0); - std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(1); - plot_3errors(error_dw[0], error_dw[1], error_dw[2], colors.at(0), stdcolor); + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(0); + std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(1); + plot_3errors(error_dw[0], error_dw[1], error_dw[2], colors.at(0), stdcolor); - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Dw Error"); - matplotlibcpp::ylabel("dw_1"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("dw_2"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("dw_3"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Dw Error"); + matplotlibcpp::ylabel("dw_1"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("dw_2"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("dw_3"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_dw[3], error_dw[4], error_dw[5], colors.at(0), stdcolor); + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_dw[3], error_dw[4], error_dw[5], colors.at(0), stdcolor); - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Dw Error"); - matplotlibcpp::ylabel("dw_3"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("dw_4"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("dw_5"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - //===================================================== + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Dw Error"); + matplotlibcpp::ylabel("dw_3"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("dw_4"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("dw_5"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); - //===================================================== - // Plot this figure - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_da[0], error_da[1], error_da[2], colors.at(0), stdcolor); + //===================================================== + //===================================================== - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Da Error"); - matplotlibcpp::ylabel("da_1"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("da_2"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("da_3"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); + // Plot this figure + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_da[0], error_da[1], error_da[2], colors.at(0), stdcolor); - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_da[3], error_da[4], error_da[5], colors.at(0), stdcolor); + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Da Error"); + matplotlibcpp::ylabel("da_1"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("da_2"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("da_3"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Da Error"); - matplotlibcpp::ylabel("da_3"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("da_4"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("da_5"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - //===================================================== + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_da[3], error_da[4], error_da[5], colors.at(0), stdcolor); - //===================================================== - // Plot this figure - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_tg[0], error_tg[1], error_tg[2], colors.at(0), stdcolor); + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Da Error"); + matplotlibcpp::ylabel("da_3"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("da_4"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("da_5"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_1"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("tg_2"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("tg_3"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); + //===================================================== + //===================================================== + // Plot this figure + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_tg[0], error_tg[1], error_tg[2], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::ylabel("tg_1"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("tg_2"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_3"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_tg[3], error_tg[4], error_tg[5], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::ylabel("tg_3"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("tg_4"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_5"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + matplotlibcpp::figure_size(800, 500); + plot_3errors(error_tg[6], error_tg[7], error_tg[8], colors.at(0), stdcolor); + + // Update the title and axis labels + matplotlibcpp::subplot(3, 1, 1); + matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::ylabel("tg_6"); + matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::ylabel("tg_7"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_8"); + matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::show(false); + + //===================================================== + //===================================================== + + // Finally plot + if (imu_model == 0) { + + // Kalibr model + // Plot this figure matplotlibcpp::figure_size(800, 500); - plot_3errors(error_tg[3], error_tg[4], error_tg[5], colors.at(0), stdcolor); + plot_3errors(error_wtoI[0], error_wtoI[1], error_wtoI[2], colors.at(0), stdcolor); // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_3"); + matplotlibcpp::title("IMU R_GyrotoI Error"); + matplotlibcpp::ylabel("x-error (deg)"); matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("tg_4"); + matplotlibcpp::ylabel("y-error (deg)"); matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("tg_5"); + matplotlibcpp::ylabel("z-error (deg)"); matplotlibcpp::xlabel("dataset time (s)"); matplotlibcpp::show(false); + } else { + + // RPNG model + // Plot this figure matplotlibcpp::figure_size(800, 500); - plot_3errors(error_tg[6], error_tg[7], error_tg[8], colors.at(0), stdcolor); + plot_3errors(error_atoI[0], error_atoI[1], error_atoI[2], colors.at(0), stdcolor); // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_6"); + matplotlibcpp::title("IMU R_AcctoI Error"); + matplotlibcpp::ylabel("x-error (deg)"); matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("tg_7"); + matplotlibcpp::ylabel("y-error (deg)"); matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("tg_8"); + matplotlibcpp::ylabel("z-error (deg)"); matplotlibcpp::xlabel("dataset time (s)"); matplotlibcpp::show(false); - //===================================================== - - if(imu_model == 0){ - // Kalibr model - //===================================================== - // Plot this figure - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_wtoI[0], error_wtoI[1], error_wtoI[2], colors.at(0), stdcolor); - - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU R_GyrotoI Error"); - matplotlibcpp::ylabel("x-error (deg)"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("y-error (deg)"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("z-error (deg)"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - //===================================================== - }else{ - // RPNG model - //===================================================== - // Plot this figure - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_atoI[0], error_atoI[1], error_atoI[2], colors.at(0), stdcolor); - - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU R_AcctoI Error"); - matplotlibcpp::ylabel("x-error (deg)"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("y-error (deg)"); - matplotlibcpp::subplot(3, 1, 3); - matplotlibcpp::ylabel("z-error (deg)"); - matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - //===================================================== - } - - + } #endif } - diff --git a/ov_eval/src/calc/ResultSimulation.h b/ov_eval/src/calc/ResultSimulation.h index 3bb0b977e..3ae736282 100644 --- a/ov_eval/src/calc/ResultSimulation.h +++ b/ov_eval/src/calc/ResultSimulation.h @@ -95,6 +95,11 @@ class ResultSimulation { */ void plot_cam_extrinsics(bool doplotting, double max_time = INFINITY); + /** + * @brief Will plot the imu intrinsic errors + * @param doplotting True if you want to display the plots + * @param max_time Max number of second we want to plot + */ void plot_imu_intrinsics(bool doploting, double max_time = INFINITY); protected: diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index bd8423ce6..80014a864 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -47,15 +47,13 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // Create the state!! state = std::make_shared(params.state_options); - // set the IMU intrinsics - + // Set the IMU intrinsics state->_imu_x_dw->set_value(params.imu_config.imu_x_dw); state->_imu_x_da->set_value(params.imu_config.imu_x_da); state->_imu_x_tg->set_value(params.imu_config.imu_x_tg); state->_imu_quat_gyrotoI->set_value(params.imu_config.imu_quat_GyrotoI); state->_imu_quat_acctoI->set_value(params.imu_config.imu_quat_AcctoI); - // Timeoffset from camera to IMU Eigen::VectorXd temp_camimu_dt; temp_camimu_dt.resize(1); diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 9f6649070..55873e77e 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -282,12 +282,15 @@ struct VioManagerOptions { masks.insert({i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)}); } } + // IMU model std::string imu_model_str = "kalibr"; - parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); - if(imu_model_str == "kalibr") imu_config.imu_model = 0; - else if(imu_model_str == "rpng") imu_config.imu_model = 1; - else { + parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); // might be redundant + if (imu_model_str == "kalibr" || imu_model_str == "calibrated") { + imu_config.imu_model = 0; + } else if (imu_model_str == "rpng") { + imu_config.imu_model = 1; + } else { PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); } @@ -303,29 +306,28 @@ struct VioManagerOptions { Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); parser->parse_external("relative_config_imu", "imu0", "Tg", Tg); - // generate the parameters we need + // TODO: error here if this returns a NaN value (i.e. invalid matrix specified) Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d R_AcctoI = R_ItoAcc.transpose(); Eigen::Matrix3d R_GyrotoI = R_ItoGyro.transpose(); // store these parameters - if(imu_config.imu_model == 0){ + if (imu_config.imu_model == 0) { // kalibr model // lower triangular of the matrix - imu_config.imu_x_dw << Dw.block<3,1>(0,0), Dw.block<2,1>(1,1), Dw(2,2); - imu_config.imu_x_da << Da.block<3,1>(0,0), Da.block<2,1>(1,1), Da(2,2); - }else{ + imu_config.imu_x_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); + imu_config.imu_x_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); + } else { // rpng model // upper triangular of the matrix - imu_config.imu_x_dw << Dw(0,0), Dw.block<2,1>(0,1), Dw.block<3,1>(0,2); - imu_config.imu_x_da << Da(0,0), Da.block<2,1>(0,1), Da.block<3,1>(0,2); + imu_config.imu_x_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2); + imu_config.imu_x_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); } - imu_config.imu_x_tg << Tg.block<3,1>(0,0), Tg.block<3,1>(0,1), Tg.block<3,1>(0,2); + imu_config.imu_x_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2); imu_config.imu_quat_GyrotoI = ov_core::rot_2_quat(R_GyrotoI); imu_config.imu_quat_AcctoI = ov_core::rot_2_quat(R_AcctoI); - } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index c87b50313..3643afecf 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -110,15 +110,18 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // Open the files of_state_est.open(filepath_est.c_str()); of_state_std.open(filepath_std.c_str()); - of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; - of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; + of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" + << std::endl; + of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" + << std::endl; // Groundtruth if we are simulating if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); of_state_gt.open(filepath_gt.c_str()); - of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; + of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" + << std::endl; } } } @@ -639,11 +642,6 @@ void ROS1Visualizer::publish_groundtruth() { quat_diff = quat_multiply(quat_st, Inv(quat_gt)); double rmse_ori = (180 / M_PI) * 2 * quat_diff.block(0, 0, 3, 1).norm(); - // - std::cout << " debug the initial values " << std::endl; - std::cout << state_ekf.block<3,1>(4,0).transpose() << std::endl; - std::cout << state_gt.block<3,1>(5,0).transpose() << std::endl; - //========================================================================== //========================================================================== diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 765bd653d..d82d46765 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -281,32 +281,21 @@ class RosVisualizerHelper { << sim->get_true_parameters().camera_extrinsics.at(i)(6) << " "; } - // Get the base IMU information of_state_gt.precision(0); of_state_gt << sim->get_true_parameters().imu_config.imu_model << " "; of_state_gt.precision(8); - of_state_gt << sim->get_true_parameters().imu_config.imu_x_dw(0) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(1) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(2) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(3) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(4) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(5) << " "; - of_state_gt << sim->get_true_parameters().imu_config.imu_x_da(0) << " " - << sim->get_true_parameters().imu_config.imu_x_da(1) << " " - << sim->get_true_parameters().imu_config.imu_x_da(2) << " " - << sim->get_true_parameters().imu_config.imu_x_da(3) << " " - << sim->get_true_parameters().imu_config.imu_x_da(4) << " " - << sim->get_true_parameters().imu_config.imu_x_da(5) << " "; - of_state_gt << sim->get_true_parameters().imu_config.imu_x_tg(0) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(1) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(2) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(3) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(4) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(5) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(6) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(7) << " " + of_state_gt << sim->get_true_parameters().imu_config.imu_x_dw(0) << " " << sim->get_true_parameters().imu_config.imu_x_dw(1) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(2) << " " << sim->get_true_parameters().imu_config.imu_x_dw(3) << " " + << sim->get_true_parameters().imu_config.imu_x_dw(4) << " " << sim->get_true_parameters().imu_config.imu_x_dw(5) << " "; + of_state_gt << sim->get_true_parameters().imu_config.imu_x_da(0) << " " << sim->get_true_parameters().imu_config.imu_x_da(1) << " " + << sim->get_true_parameters().imu_config.imu_x_da(2) << " " << sim->get_true_parameters().imu_config.imu_x_da(3) << " " + << sim->get_true_parameters().imu_config.imu_x_da(4) << " " << sim->get_true_parameters().imu_config.imu_x_da(5) << " "; + of_state_gt << sim->get_true_parameters().imu_config.imu_x_tg(0) << " " << sim->get_true_parameters().imu_config.imu_x_tg(1) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(2) << " " << sim->get_true_parameters().imu_config.imu_x_tg(3) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(4) << " " << sim->get_true_parameters().imu_config.imu_x_tg(5) << " " + << sim->get_true_parameters().imu_config.imu_x_tg(6) << " " << sim->get_true_parameters().imu_config.imu_x_tg(7) << " " << sim->get_true_parameters().imu_config.imu_x_tg(8) << " "; of_state_gt << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(0) << " " << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(1) << " " @@ -408,78 +397,50 @@ class RosVisualizerHelper { } } - - /// base IMU parameters + // imu intrinsics: what model we are using of_state_est.precision(0); of_state_est << state->_options.imu_model << " "; of_state_est.precision(8); - of_state_std.precision(0); of_state_std << state->_options.imu_model << " "; of_state_std.precision(8); - // imu intrinsics - // dw - of_state_est << state->_imu_x_dw->value()(0) << " " - << state->_imu_x_dw->value()(1) << " " - << state->_imu_x_dw->value()(2) << " " - << state->_imu_x_dw->value()(3) << " " - << state->_imu_x_dw->value()(4) << " " - << state->_imu_x_dw->value()(5) << " "; + // imu intrinsics: dw + of_state_est << state->_imu_x_dw->value()(0) << " " << state->_imu_x_dw->value()(1) << " " << state->_imu_x_dw->value()(2) << " " + << state->_imu_x_dw->value()(3) << " " << state->_imu_x_dw->value()(4) << " " << state->_imu_x_dw->value()(5) << " "; if (state->_options.do_calib_imu_intrinsics) { int index_dw = state->_imu_x_dw->id(); - of_state_std << std::sqrt(cov(index_dw + 0, index_dw + 0)) << " " - << std::sqrt(cov(index_dw + 1, index_dw + 1)) << " " - << std::sqrt(cov(index_dw + 2, index_dw + 2)) << " " - << std::sqrt(cov(index_dw + 3, index_dw + 3)) << " " - << std::sqrt(cov(index_dw + 4, index_dw + 4)) << " " - << std::sqrt(cov(index_dw + 5, index_dw + 5)) << " "; + of_state_std << std::sqrt(cov(index_dw + 0, index_dw + 0)) << " " << std::sqrt(cov(index_dw + 1, index_dw + 1)) << " " + << std::sqrt(cov(index_dw + 2, index_dw + 2)) << " " << std::sqrt(cov(index_dw + 3, index_dw + 3)) << " " + << std::sqrt(cov(index_dw + 4, index_dw + 4)) << " " << std::sqrt(cov(index_dw + 5, index_dw + 5)) << " "; } else { of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; } - // da - of_state_est << state->_imu_x_da->value()(0) << " " - << state->_imu_x_da->value()(1) << " " - << state->_imu_x_da->value()(2) << " " - << state->_imu_x_da->value()(3) << " " - << state->_imu_x_da->value()(4) << " " - << state->_imu_x_da->value()(5) << " "; + // imu intrinsics: da + of_state_est << state->_imu_x_da->value()(0) << " " << state->_imu_x_da->value()(1) << " " << state->_imu_x_da->value()(2) << " " + << state->_imu_x_da->value()(3) << " " << state->_imu_x_da->value()(4) << " " << state->_imu_x_da->value()(5) << " "; if (state->_options.do_calib_imu_intrinsics) { int index_da = state->_imu_x_da->id(); - of_state_std << std::sqrt(cov(index_da + 0, index_da + 0)) << " " - << std::sqrt(cov(index_da + 1, index_da + 1)) << " " - << std::sqrt(cov(index_da + 2, index_da + 2)) << " " - << std::sqrt(cov(index_da + 3, index_da + 3)) << " " - << std::sqrt(cov(index_da + 4, index_da + 4)) << " " - << std::sqrt(cov(index_da + 5, index_da + 5)) << " "; + of_state_std << std::sqrt(cov(index_da + 0, index_da + 0)) << " " << std::sqrt(cov(index_da + 1, index_da + 1)) << " " + << std::sqrt(cov(index_da + 2, index_da + 2)) << " " << std::sqrt(cov(index_da + 3, index_da + 3)) << " " + << std::sqrt(cov(index_da + 4, index_da + 4)) << " " << std::sqrt(cov(index_da + 5, index_da + 5)) << " "; } else { of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; } - - // tg - of_state_est << state->_imu_x_tg->value()(0) << " " - << state->_imu_x_tg->value()(1) << " " - << state->_imu_x_tg->value()(2) << " " - << state->_imu_x_tg->value()(3) << " " - << state->_imu_x_tg->value()(4) << " " - << state->_imu_x_tg->value()(5) << " " - << state->_imu_x_tg->value()(6) << " " - << state->_imu_x_tg->value()(7) << " " - << state->_imu_x_tg->value()(8) << " "; + // imu intrinsics: tg + of_state_est << state->_imu_x_tg->value()(0) << " " << state->_imu_x_tg->value()(1) << " " << state->_imu_x_tg->value()(2) << " " + << state->_imu_x_tg->value()(3) << " " << state->_imu_x_tg->value()(4) << " " << state->_imu_x_tg->value()(5) << " " + << state->_imu_x_tg->value()(6) << " " << state->_imu_x_tg->value()(7) << " " << state->_imu_x_tg->value()(8) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { int index_tg = state->_imu_x_tg->id(); - of_state_std << std::sqrt(cov(index_tg + 0, index_tg + 0)) << " " - << std::sqrt(cov(index_tg + 1, index_tg + 1)) << " " - << std::sqrt(cov(index_tg + 2, index_tg + 2)) << " " - << std::sqrt(cov(index_tg + 3, index_tg + 3)) << " " - << std::sqrt(cov(index_tg + 4, index_tg + 4)) << " " - << std::sqrt(cov(index_tg + 5, index_tg + 5)) << " " - << std::sqrt(cov(index_tg + 6, index_tg + 6)) << " " - << std::sqrt(cov(index_tg + 7, index_tg + 7)) << " " + of_state_std << std::sqrt(cov(index_tg + 0, index_tg + 0)) << " " << std::sqrt(cov(index_tg + 1, index_tg + 1)) << " " + << std::sqrt(cov(index_tg + 2, index_tg + 2)) << " " << std::sqrt(cov(index_tg + 3, index_tg + 3)) << " " + << std::sqrt(cov(index_tg + 4, index_tg + 4)) << " " << std::sqrt(cov(index_tg + 5, index_tg + 5)) << " " + << std::sqrt(cov(index_tg + 6, index_tg + 6)) << " " << std::sqrt(cov(index_tg + 7, index_tg + 7)) << " " << std::sqrt(cov(index_tg + 8, index_tg + 8)) << " "; } else { of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; @@ -487,33 +448,24 @@ class RosVisualizerHelper { of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; } - // R_gyrotoI - of_state_est << state->_imu_quat_gyrotoI->value()(0) << " " - << state->_imu_quat_gyrotoI->value()(1) << " " - << state->_imu_quat_gyrotoI->value()(2) << " " - << state->_imu_quat_gyrotoI->value()(3) << " "; + // imu intrinsics: kalibr R_gyrotoI + of_state_est << state->_imu_quat_gyrotoI->value()(0) << " " << state->_imu_quat_gyrotoI->value()(1) << " " + << state->_imu_quat_gyrotoI->value()(2) << " " << state->_imu_quat_gyrotoI->value()(3) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { int index_wtoI = state->_imu_quat_gyrotoI->id(); - of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " - << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " " - << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " - << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " "; + of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " " + << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " "; } else { of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; } - - // R_acctoI - of_state_est << state->_imu_quat_acctoI->value()(0) << " " - << state->_imu_quat_acctoI->value()(1) << " " - << state->_imu_quat_acctoI->value()(2) << " " - << state->_imu_quat_acctoI->value()(3) << " "; + // imu intrinsics: rpng R_acctoI + of_state_est << state->_imu_quat_acctoI->value()(0) << " " << state->_imu_quat_acctoI->value()(1) << " " + << state->_imu_quat_acctoI->value()(2) << " " << state->_imu_quat_acctoI->value()(3) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { int index_atoI = state->_imu_quat_acctoI->id(); - of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " - << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " " - << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " - << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " "; + of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " " + << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " "; } else { of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; } diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 880214e81..3c4f44918 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -228,30 +228,28 @@ void Simulator::perturb_parameters(std::mt19937 gen_state, VioManagerOptions &pa } // perturb the imu intrinsics - for(int j=0; j<6; j++){ + for (int j = 0; j < 6; j++) { params_.imu_config.imu_x_dw(j) += 0.004 * w(gen_state); params_.imu_config.imu_x_da(j) += 0.004 * w(gen_state); } // if we need to perturb gravity sensitivity - for(int j=0; j<5; j++){ - if(params_.state_options.do_calib_imu_g_sensitivity){ + for (int j = 0; j < 5; j++) { + if (params_.state_options.do_calib_imu_g_sensitivity) { params_.imu_config.imu_x_tg(j) += 0.004 * w(gen_state); } } - // depends on Kalibr model or RPNG model - if(params_.state_options.imu_model == 0){ + // perturb imu intrinsics (depends on model type) + if (params_.state_options.imu_model == 0) { Eigen::Vector3d w_vec; w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); params_.imu_config.imu_quat_GyrotoI = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.imu_quat_GyrotoI)); - }else{ + } else { Eigen::Vector3d w_vec; w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); params_.imu_config.imu_quat_AcctoI = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.imu_quat_AcctoI)); } - - } bool Simulator::get_state(double desired_time, Eigen::Matrix &imustate) { @@ -332,14 +330,12 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto Eigen::Vector3d accel_inI = R_GtoI * (a_IinG + gravity); Eigen::Vector3d omega_inI = w_IinI; - // get the readings with the imu intrinsics + // Get the readings with the imu intrinsic "distortion" Eigen::Matrix3d Tg = params.imu_config.Tg(); Eigen::Matrix3d Tw = params.imu_config.Tw(); Eigen::Matrix3d Ta = params.imu_config.Ta(); Eigen::Vector3d omega_inw = Tw * params.imu_config.R_ItoGyro() * omega_inI + Tg * accel_inI; - Eigen::Vector3d accel_ina = Ta * params.imu_config.R_ItoAcc() * accel_inI; - - + Eigen::Vector3d accel_ina = Ta * params.imu_config.R_ItoAcc() * accel_inI; // Now add noise to these measurements double dt = 1.0 / params.sim_freq_imu; diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index d2b12f443..b62a8afaa 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -64,8 +64,8 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest // Q_summed = Phi_i*Q_summed*Phi_i^T + Q_i // After summing we can multiple the total phi to get the updated covariance // We will then add the noise to the IMU portion of the state - Eigen::MatrixXd Phi_summed = Eigen::MatrixXd::Identity(state->imu_intrinsic_size()+15,state->imu_intrinsic_size()+15); - Eigen::MatrixXd Qd_summed = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15,state->imu_intrinsic_size()+15); + Eigen::MatrixXd Phi_summed = Eigen::MatrixXd::Identity(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); + Eigen::MatrixXd Qd_summed = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); double dt_summed = 0; // Loop through all IMU messages, and use them to move the state forward in time @@ -90,33 +90,32 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest } } -// // Last angular velocity (used for cloning when estimating time offset) -// Eigen::Matrix last_w = Eigen::Matrix::Zero(); -// if (prop_data.size() > 1) -// last_w = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); -// else if (!prop_data.empty()) -// last_w = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); + // // Last angular velocity (used for cloning when estimating time offset) + // Eigen::Matrix last_w = Eigen::Matrix::Zero(); + // if (prop_data.size() > 1) + // last_w = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); + // else if (!prop_data.empty()) + // last_w = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); // Last angular velocity (used for cloning when estimating time offset) // Remember to correct them before we store them - Eigen::Matrix last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size()-1).am - state->_imu->bias_a()); - Eigen::Matrix last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; - Eigen::Matrix last_w = state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size()-1).wm - state->_imu->bias_g() - state->Tg()*last_a); - - + Eigen::Matrix last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); + Eigen::Matrix last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; + Eigen::Matrix last_w = + state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); // Do the update to the covariance with our "summed" state transition and IMU noise addition... std::vector> Phi_order; Phi_order.push_back(state->_imu); - if(state->_options.do_calib_imu_intrinsics){ + if (state->_options.do_calib_imu_intrinsics) { Phi_order.push_back(state->_imu_x_dw); Phi_order.push_back(state->_imu_x_da); - if(state->_options.do_calib_imu_g_sensitivity){ + if (state->_options.do_calib_imu_g_sensitivity) { Phi_order.push_back(state->_imu_x_tg); } - if(state->_options.imu_model == 0){ + if (state->_options.imu_model == 0) { Phi_order.push_back(state->_imu_quat_gyrotoI); - }else{ + } else { Phi_order.push_back(state->_imu_quat_acctoI); } } @@ -167,7 +166,7 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times Eigen::Matrix a_hat2 = data_plus.am - state->_imu->bias_a(); Eigen::Matrix a_hat = a_hat1; if (state->_options.imu_avg) { - a_hat = .5*(a_hat1+a_hat2); + a_hat = .5 * (a_hat1 + a_hat2); } // Imu acc readings in raw frame Eigen::Vector3d a_uncorrected = a_hat; @@ -185,8 +184,9 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; Eigen::Matrix w_hat = w_hat1; if (state->_options.imu_avg) { - w_hat = .5*(w_hat1+w_hat2); + w_hat = .5 * (w_hat1 + w_hat2); } + // Imu readings in raw frame Eigen::Vector3d w_uncorrected = w_hat; @@ -196,7 +196,7 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; // Pre-compute some analytical values - Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3,18); + Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); if (state->_options.use_analytic_integration) { compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); } @@ -204,8 +204,8 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if(state->_options.use_analytic_integration) - predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); + if (state->_options.use_analytic_integration) + predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); else if (state->_options.use_rk4_integration) predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); else @@ -368,7 +368,7 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core Eigen::Matrix a_hat2 = data_plus.am - state->_imu->bias_a(); Eigen::Matrix a_hat = a_hat1; if (state->_options.imu_avg) { - a_hat = .5*(a_hat1+a_hat2); + a_hat = .5 * (a_hat1 + a_hat2); } // Imu acc readings in raw frame Eigen::Vector3d a_uncorrected = a_hat; @@ -377,7 +377,6 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; - // gravity sensitivity correction Eigen::Matrix gravity_correction1 = state->Tg() * a_hat1; Eigen::Matrix gravity_correction2 = state->Tg() * a_hat2; @@ -387,7 +386,7 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; Eigen::Matrix w_hat = w_hat1; if (state->_options.imu_avg) { - w_hat = .5*(w_hat1+w_hat2); + w_hat = .5 * (w_hat1 + w_hat2); } // Imu readings in raw frame Eigen::Vector3d w_uncorrected = w_hat; @@ -398,7 +397,7 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; // Pre-compute some analytical values - Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3,18); + Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); if (state->_options.use_analytic_integration) { compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); } @@ -406,23 +405,21 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if(state->_options.use_analytic_integration) - predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); + if (state->_options.use_analytic_integration) + predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); else if (state->_options.use_rk4_integration) predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); else predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); // Allocate state transition and noise Jacobian - F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); - Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15,12); + F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, 12); - if(state->_options.use_analytic_integration || state->_options.use_rk4_integration){ - compute_F_and_G_analytic(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, - new_q, new_v, new_p, Xi_sum, F, G); + if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { + compute_F_and_G_analytic(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G); } else { - compute_F_and_G_discrete(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, - new_q, new_v, new_p, F, G); + compute_F_and_G_discrete(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, F, G); } // Construct our discrete noise covariance matrix @@ -435,7 +432,7 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core Qc.block(9, 9, 3, 3) = _imu_config.sigma_ab_2 / dt * Eigen::Matrix::Identity(); // Compute the noise injected into the state over the interval - Qd = Eigen::MatrixXd::Zero(state->imu_intrinsic_size()+15, state->imu_intrinsic_size()+15); + Qd = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); Qd = G * Qc * G.transpose(); Qd = 0.5 * (Qd + Qd.transpose()); @@ -448,23 +445,27 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core state->_imu->set_fej(imu_x); } -void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, - const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, - const Eigen::Matrix &Xi_sum, - Eigen::MatrixXd &F, Eigen::MatrixXd &G) { +void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, + const Eigen::Vector3d &a_hat, const Eigen::Vector3d &w_uncorrected, + const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, + const Eigen::Vector3d &new_p, const Eigen::Matrix &Xi_sum, Eigen::MatrixXd &F, + Eigen::MatrixXd &G) { //============================================================ //============================================================ // Get the locations of each entry of the imu state int local_size = 0; - int th_id = local_size; local_size += state->_imu->q()->size(); - int p_id = local_size; local_size += state->_imu->p()->size(); - int v_id = local_size; local_size += state->_imu->v()->size(); - int bg_id = local_size; local_size += state->_imu->bg()->size(); - int ba_id = local_size; local_size += state->_imu->ba()->size(); + int th_id = local_size; + local_size += state->_imu->q()->size(); + int p_id = local_size; + local_size += state->_imu->p()->size(); + int v_id = local_size; + local_size += state->_imu->v()->size(); + int bg_id = local_size; + local_size += state->_imu->bg()->size(); + int ba_id = local_size; + local_size += state->_imu->ba()->size(); // If we are doing calibration, we can define their "local" id in the state transition int Dw_id = -1; @@ -472,20 +473,20 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d int Tg_id = -1; int th_atoI_id = -1; int th_wtoI_id = -1; - if(state->_options.do_calib_imu_intrinsics) { + if (state->_options.do_calib_imu_intrinsics) { Dw_id = local_size; local_size += state->_imu_x_dw->size(); Da_id = local_size; local_size += state->_imu_x_da->size(); - if(state->_options.do_calib_imu_g_sensitivity){ + if (state->_options.do_calib_imu_g_sensitivity) { Tg_id = local_size; local_size += state->_imu_x_tg->size(); } - if(state->_options.imu_model == 0){ + if (state->_options.imu_model == 0) { // Kalibr model th_wtoI_id = local_size; local_size += state->_imu_quat_gyrotoI->size(); - }else{ + } else { // RPNG model th_atoI_id = local_size; local_size += state->_imu_quat_acctoI->size(); @@ -497,125 +498,128 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix R_k = state->_imu->Rot(); - Eigen::Matrix v_k = state->_imu->vel(); - Eigen::Matrix p_k = state->_imu->pos(); + Eigen::Matrix R_k = state->_imu->Rot(); + Eigen::Matrix v_k = state->_imu->vel(); + Eigen::Matrix p_k = state->_imu->pos(); - if(state->_options.do_fej){ + if (state->_options.do_fej) { R_k = state->_imu->Rot_fej(); v_k = state->_imu->vel_fej(); p_k = state->_imu->pos_fej(); } - Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q)*R_k.transpose(); - - Eigen::Matrix Da = state->Da(); - Eigen::Matrix Dw = state->Dw(); - Eigen::Matrix Tg = state->Tg(); - Eigen::Matrix R_atoI = state->R_AcctoI(); - Eigen::Matrix R_wtoI = state->R_GyrotoI(); - Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; - Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected;// contains the gravity correction already - - Eigen::Matrix3d Xi_1 = Xi_sum.block<3,3>(0,3); - Eigen::Matrix3d Xi_2 = Xi_sum.block<3,3>(0,6); - Eigen::Matrix3d Jr = Xi_sum.block<3,3>(0,9); - Eigen::Matrix3d Xi_3 = Xi_sum.block<3,3>(0,12); - Eigen::Matrix3d Xi_4 = Xi_sum.block<3,3>(0,15); + Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); + + Eigen::Matrix Da = state->Da(); + Eigen::Matrix Dw = state->Dw(); + Eigen::Matrix Tg = state->Tg(); + Eigen::Matrix R_atoI = state->R_AcctoI(); + Eigen::Matrix R_wtoI = state->R_GyrotoI(); + Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; + Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected; // contains the gravity correction already + + Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3); + Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6); + Eigen::Matrix3d Jr = Xi_sum.block<3, 3>(0, 9); + Eigen::Matrix3d Xi_3 = Xi_sum.block<3, 3>(0, 12); + Eigen::Matrix3d Xi_4 = Xi_sum.block<3, 3>(0, 15); // for th - F.block<3,3>(th_id, th_id) = dR_ktok1; - F.block<3,3>(p_id, th_id).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); - F.block<3,3>(v_id, th_id).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); + F.block<3, 3>(th_id, th_id) = dR_ktok1; + F.block<3, 3>(p_id, th_id).noalias() = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); + F.block<3, 3>(v_id, th_id).noalias() = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); // for p - F.block<3,3>(p_id, p_id).setIdentity(); + F.block<3, 3>(p_id, p_id).setIdentity(); // for v - F.block<3,3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; - F.block<3,3>(v_id, v_id).setIdentity(); + F.block<3, 3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; + F.block<3, 3>(v_id, v_id).setIdentity(); // for bg - F.block<3,3>(th_id, bg_id).noalias() = -Jr * dt * R_wtoI * Dw; - F.block<3,3>(p_id, bg_id) = R_k.transpose() * Xi_4 * R_wtoI* Dw; - F.block<3,3>(v_id, bg_id) = R_k.transpose() * Xi_3 * R_wtoI* Dw; - F.block<3,3>(bg_id, bg_id).setIdentity(); + F.block<3, 3>(th_id, bg_id).noalias() = -Jr * dt * R_wtoI * Dw; + F.block<3, 3>(p_id, bg_id) = R_k.transpose() * Xi_4 * R_wtoI * Dw; + F.block<3, 3>(v_id, bg_id) = R_k.transpose() * Xi_3 * R_wtoI * Dw; + F.block<3, 3>(bg_id, bg_id).setIdentity(); // for ba - F.block<3,3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - F.block<3,3>(p_id, ba_id) = -R_k.transpose()*(Xi_2+Xi_4*R_wtoI*Dw*Tg)*R_atoI*Da; - F.block<3,3>(v_id, ba_id) = -R_k.transpose()*(Xi_1+Xi_3*R_wtoI*Dw*Tg)*R_atoI*Da; - F.block<3,3>(ba_id, ba_id).setIdentity(); - + F.block<3, 3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block<3, 3>(p_id, ba_id) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; + F.block<3, 3>(v_id, ba_id) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; + F.block<3, 3>(ba_id, ba_id).setIdentity(); // begin to add the state transition matrix for the omega intrinsics part - if(Dw_id != -1){ - F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); - F.block(p_id, Dw_id, 3, state->_imu_x_dw->size()) = - R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state,w_uncorrected); - F.block(v_id, Dw_id, 3, state->_imu_x_dw->size()) = - R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state,w_uncorrected); + if (Dw_id != -1) { + F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); + F.block(p_id, Dw_id, 3, state->_imu_x_dw->size()) = -R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state, w_uncorrected); + F.block(v_id, Dw_id, 3, state->_imu_x_dw->size()) = -R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state, w_uncorrected); F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); } // begin to add the state transition matrix for the acc intrinsics part - if(Da_id != -1){ + if (Da_id != -1) { F.block(Da_id, Da_id, state->_imu_x_da->size(), state->_imu_x_da->size()).setIdentity(); - F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * compute_H_Da(state,w_uncorrected); - F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * ( Xi_2 + Xi_4 * R_wtoI * Dw * Tg ) * R_atoI * compute_H_Da(state,a_uncorrected); - F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg )* R_atoI * compute_H_Da(state,a_uncorrected); + F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * compute_H_Da(state, w_uncorrected); + F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = + R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * compute_H_Da(state, a_uncorrected); + F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = + R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * compute_H_Da(state, a_uncorrected); } // add the state trasition matrix of the tg part - if(Tg_id != -1){ - F.block(Tg_id,Tg_id,state->_imu_x_tg->size(),state->_imu_x_tg->size()).setIdentity(); - F.block(th_id,Tg_id,3,state->_imu_x_tg->size()) = -Jr*dt*R_wtoI*Dw*compute_H_Tg(state,a_k); - F.block(p_id,Tg_id,3,state->_imu_x_tg->size())=R_k.transpose()*Xi_4*R_wtoI*Dw*compute_H_Tg(state,a_k); - F.block(v_id,Tg_id,3,state->_imu_x_tg->size())=R_k.transpose()*Xi_3*R_wtoI*Dw*compute_H_Tg(state,a_k); + if (Tg_id != -1) { + F.block(Tg_id, Tg_id, state->_imu_x_tg->size(), state->_imu_x_tg->size()).setIdentity(); + F.block(th_id, Tg_id, 3, state->_imu_x_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(p_id, Tg_id, 3, state->_imu_x_tg->size()) = R_k.transpose() * Xi_4 * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(v_id, Tg_id, 3, state->_imu_x_tg->size()) = R_k.transpose() * Xi_3 * R_wtoI * Dw * compute_H_Tg(state, a_k); } // begin to add the state transition matrix for the acctoI part - if(th_atoI_id != -1){ - F.block<3,3>(th_atoI_id, th_atoI_id).setIdentity(); - F.block<3,3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); - F.block<3,3>(p_id, th_atoI_id) = R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); - F.block<3,3>(v_id, th_atoI_id) = R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) *ov_core::skew_x(a_k); + if (th_atoI_id != -1) { + F.block<3, 3>(th_atoI_id, th_atoI_id).setIdentity(); + F.block<3, 3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block<3, 3>(p_id, th_atoI_id) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); + F.block<3, 3>(v_id, th_atoI_id) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); } // begin to add the state transition matrix for the gyrotoI part - if(th_wtoI_id != -1){ - F.block<3,3>(th_wtoI_id, th_wtoI_id).setIdentity(); - F.block<3,3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); - F.block<3,3>(p_id, th_wtoI_id) = - R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); - F.block<3,3>(v_id, th_wtoI_id) = - R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); + if (th_wtoI_id != -1) { + F.block<3, 3>(th_wtoI_id, th_wtoI_id).setIdentity(); + F.block<3, 3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); + F.block<3, 3>(p_id, th_wtoI_id) = -R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); + F.block<3, 3>(v_id, th_wtoI_id) = -R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); } // construct the G part - G.block<3,3>(th_id, 0) = -Jr * dt *R_wtoI * Dw; - G.block<3,3>(p_id, 0) = R_k.transpose() * Xi_4 *R_wtoI * Dw; - G.block<3,3>(v_id, 0) = R_k.transpose() * Xi_3 *R_wtoI * Dw; - G.block<3,3>(th_id,3) = Jr*dt*R_wtoI*Dw*Tg*R_atoI*Da; - G.block<3,3>(p_id, 3) = -R_k.transpose() * (Xi_2+Xi_4*R_wtoI*Dw*Tg) * R_atoI * Da; - G.block<3,3>(v_id, 3) = -R_k.transpose() * (Xi_1+Xi_3*R_wtoI*Dw*Tg) * R_atoI * Da; - G.block<3,3>(bg_id, 6) = dt*Eigen::Matrix::Identity(); - G.block<3,3>(ba_id, 9) = dt*Eigen::Matrix::Identity(); - + G.block<3, 3>(th_id, 0) = -Jr * dt * R_wtoI * Dw; + G.block<3, 3>(p_id, 0) = R_k.transpose() * Xi_4 * R_wtoI * Dw; + G.block<3, 3>(v_id, 0) = R_k.transpose() * Xi_3 * R_wtoI * Dw; + G.block<3, 3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block<3, 3>(p_id, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; + G.block<3, 3>(v_id, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; + G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix::Identity(); + G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix::Identity(); } - -void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, - const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, - Eigen::MatrixXd &F, Eigen::MatrixXd &G) { +void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, + const Eigen::Vector3d &a_hat, const Eigen::Vector3d &w_uncorrected, + const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, + const Eigen::Vector3d &new_p, Eigen::MatrixXd &F, Eigen::MatrixXd &G) { //============================================================ //============================================================ // Get the locations of each entry of the imu state int local_size = 0; - int th_id = local_size; local_size += state->_imu->q()->size(); - int p_id = local_size; local_size += state->_imu->p()->size(); - int v_id = local_size; local_size += state->_imu->v()->size(); - int bg_id = local_size; local_size += state->_imu->bg()->size(); - int ba_id = local_size; local_size += state->_imu->ba()->size(); + int th_id = local_size; + local_size += state->_imu->q()->size(); + int p_id = local_size; + local_size += state->_imu->p()->size(); + int v_id = local_size; + local_size += state->_imu->v()->size(); + int bg_id = local_size; + local_size += state->_imu->bg()->size(); + int ba_id = local_size; + local_size += state->_imu->ba()->size(); // If we are doing calibration, we can define their "local" id in the state transition int Dw_id = -1; @@ -623,129 +627,118 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d int Tg_id = -1; int th_atoI_id = -1; int th_wtoI_id = -1; - if(state->_options.do_calib_imu_intrinsics) { + if (state->_options.do_calib_imu_intrinsics) { Dw_id = local_size; local_size += state->_imu_x_dw->size(); Da_id = local_size; local_size += state->_imu_x_da->size(); - if(state->_options.do_calib_imu_g_sensitivity){ + if (state->_options.do_calib_imu_g_sensitivity) { Tg_id = local_size; local_size += state->_imu_x_tg->size(); } - if(state->_options.imu_model == 0){ + if (state->_options.imu_model == 0) { // Kalibr model th_wtoI_id = local_size; local_size += state->_imu_quat_gyrotoI->size(); - }else{ + } else { // RPNG model th_atoI_id = local_size; local_size += state->_imu_quat_acctoI->size(); } } - //============================================================ //============================================================ // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix R_k = state->_imu->Rot(); - Eigen::Matrix v_k = state->_imu->vel(); - Eigen::Matrix p_k = state->_imu->pos(); + Eigen::Matrix R_k = state->_imu->Rot(); + Eigen::Matrix v_k = state->_imu->vel(); + Eigen::Matrix p_k = state->_imu->pos(); - if(state->_options.do_fej){ + if (state->_options.do_fej) { R_k = state->_imu->Rot_fej(); v_k = state->_imu->vel_fej(); p_k = state->_imu->pos_fej(); } - Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q)*R_k.transpose(); - + Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix Da = state->Da(); - Eigen::Matrix Dw = state->Dw(); - Eigen::Matrix Tg = state->Tg(); - Eigen::Matrix R_atoI = state->R_AcctoI(); - Eigen::Matrix R_wtoI = state->R_GyrotoI(); - Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; - Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected;// contains the gravity correction already - Eigen::Matrix Jr = Jr_so3(w_k * dt); + Eigen::Matrix Da = state->Da(); + Eigen::Matrix Dw = state->Dw(); + Eigen::Matrix Tg = state->Tg(); + Eigen::Matrix R_atoI = state->R_AcctoI(); + Eigen::Matrix R_wtoI = state->R_GyrotoI(); + Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; + Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected; // contains the gravity correction already + Eigen::Matrix Jr = Jr_so3(w_k * dt); // for theta - F.block<3,3>(th_id, th_id) = dR_ktok1; - //F.block(th_id, bg_id, 3, 3).noalias() = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; - F.block<3,3>(th_id, bg_id).noalias() = - Jr * dt * R_wtoI * Dw; - F.block<3,3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block<3, 3>(th_id, th_id) = dR_ktok1; + // F.block(th_id, bg_id, 3, 3).noalias() = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; + F.block<3, 3>(th_id, bg_id).noalias() = -Jr * dt * R_wtoI * Dw; + F.block<3, 3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; // for position - F.block<3,3>(p_id, th_id).noalias() = -skew_x(new_p-p_k-v_k*dt+0.5*_gravity*dt*dt)*R_k.transpose(); - F.block<3,3>(p_id, p_id).setIdentity(); - F.block<3,3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; - F.block<3,3>(p_id, ba_id) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + F.block<3, 3>(p_id, th_id).noalias() = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); + F.block<3, 3>(p_id, p_id).setIdentity(); + F.block<3, 3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; + F.block<3, 3>(p_id, ba_id) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; // for velocity - F.block<3,3>(v_id, th_id).noalias() = -skew_x(new_v-v_k+_gravity*dt)*R_k.transpose(); - F.block<3,3>(v_id, v_id).setIdentity(); - F.block<3,3>(v_id, ba_id) = -R_k.transpose() * dt * R_atoI * Da; + F.block<3, 3>(v_id, th_id).noalias() = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); + F.block<3, 3>(v_id, v_id).setIdentity(); + F.block<3, 3>(v_id, ba_id) = -R_k.transpose() * dt * R_atoI * Da; // for bg - F.block<3,3>(bg_id, bg_id).setIdentity(); + F.block<3, 3>(bg_id, bg_id).setIdentity(); // for ba - F.block<3,3>(ba_id, ba_id).setIdentity(); - - + F.block<3, 3>(ba_id, ba_id).setIdentity(); // begin to add the state transition matrix for the omega intrinsics part - if(Dw_id != -1) { + if (Dw_id != -1) { F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); - F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state,w_uncorrected); + F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); } // begin to add the state transition matrix for the acc intrinsics part - if(Da_id != -1) { - F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI*Tg*R_atoI*compute_H_Da(state,a_uncorrected); - F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * compute_H_Da(state,a_uncorrected); - F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * dt * R_atoI * compute_H_Da(state,a_uncorrected); + if (Da_id != -1) { + F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI * Tg * R_atoI * compute_H_Da(state, a_uncorrected); + F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * compute_H_Da(state, a_uncorrected); + F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * dt * R_atoI * compute_H_Da(state, a_uncorrected); F.block(Da_id, Da_id, state->_imu_x_da->size(), state->_imu_x_da->size()).setIdentity(); } // begin to add the state transition matrix for the acc intrinsics part - if(th_atoI_id != -1) { - F.block<3,3>(th_atoI_id, th_atoI_id).setIdentity(); - F.block<3,3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); - F.block<3,3>(p_id, th_atoI_id) = 0.5 * R_k.transpose() * dt * dt *ov_core::skew_x(a_k); - F.block<3,3>(v_id, th_atoI_id) = R_k.transpose() * dt *ov_core::skew_x(a_k); + if (th_atoI_id != -1) { + F.block<3, 3>(th_atoI_id, th_atoI_id).setIdentity(); + F.block<3, 3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block<3, 3>(p_id, th_atoI_id) = 0.5 * R_k.transpose() * dt * dt * ov_core::skew_x(a_k); + F.block<3, 3>(v_id, th_atoI_id) = R_k.transpose() * dt * ov_core::skew_x(a_k); } // begin to add the state transition matrix for the gyro intrinsics part - if(th_wtoI_id != -1) { - F.block<3,3>(th_wtoI_id, th_wtoI_id).setIdentity(); - F.block<3,3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); + if (th_wtoI_id != -1) { + F.block<3, 3>(th_wtoI_id, th_wtoI_id).setIdentity(); + F.block<3, 3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); } // begin to add the state transition matrix for the gravity sensitivity part - if(Tg_id != -1){ - F.block(Tg_id, Tg_id, state->_imu_x_tg->size(),state->_imu_x_tg->size()).setIdentity(); - F.block(th_id, Tg_id, 3, state->_imu_x_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state,a_k); + if (Tg_id != -1) { + F.block(Tg_id, Tg_id, state->_imu_x_tg->size(), state->_imu_x_tg->size()).setIdentity(); + F.block(th_id, Tg_id, 3, state->_imu_x_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); } // Noise jacobian - G.block<3,3>(th_id, 0) = -Jr * dt * R_wtoI * Dw; - G.block<3,3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - G.block<3,3>(v_id, 3) = -R_k.transpose() * dt * R_atoI * Da; - G.block<3,3>(p_id, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; - G.block<3,3>(bg_id, 6) = dt*Eigen::Matrix::Identity(); - G.block<3,3>(ba_id, 9) = dt*Eigen::Matrix::Identity(); - + G.block<3, 3>(th_id, 0) = -Jr * dt * R_wtoI * Dw; + G.block<3, 3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block<3, 3>(v_id, 3) = -R_k.transpose() * dt * R_atoI * Da; + G.block<3, 3>(p_id, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix::Identity(); + G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix::Identity(); } - - - - - - void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { @@ -780,35 +773,29 @@ void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, new_p = state->_imu->pos() + state->_imu->vel() * dt + 0.5 * R_Gtoi.transpose() * a_hat * dt * dt - 0.5 * _gravity * dt * dt; } - -void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, +void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, - Eigen::Matrix& Xi_sum) { + Eigen::Matrix &Xi_sum) { // Pre-compute things - Eigen::Matrix R_Gtok = state->_imu->Rot(); + Eigen::Matrix R_Gtok = state->_imu->Rot(); // get the pre-computed value - Eigen::Matrix3d R_k1_to_k = Xi_sum.block<3,3>(0,0); - Eigen::Matrix3d Xi_1 = Xi_sum.block<3,3>(0,3); - Eigen::Matrix3d Xi_2 = Xi_sum.block<3,3>(0,6); + Eigen::Matrix3d R_k1_to_k = Xi_sum.block<3, 3>(0, 0); + Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3); + Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6); // use the precomputed value to get the new state - Eigen::Matrix q_k_to_k1 = ov_core::rot_2_quat(R_k1_to_k.transpose()); + Eigen::Matrix q_k_to_k1 = ov_core::rot_2_quat(R_k1_to_k.transpose()); new_q = ov_core::quat_multiply(q_k_to_k1, state->_imu->quat()); // Velocity: just the acceleration in the local frame, minus global gravity - new_v = state->_imu->vel() + R_Gtok.transpose()* Xi_1 *a_hat - _gravity*dt; + new_v = state->_imu->vel() + R_Gtok.transpose() * Xi_1 * a_hat - _gravity * dt; // Position: just velocity times dt, with the acceleration integrated twice - new_p = state->_imu->pos() + state->_imu->vel()*dt + R_Gtok.transpose()*Xi_2*a_hat - 0.5*_gravity*dt*dt; - - + new_p = state->_imu->pos() + state->_imu->vel() * dt + R_Gtok.transpose() * Xi_2 * a_hat - 0.5 * _gravity * dt * dt; } - - void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { @@ -890,113 +877,103 @@ void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v; } - void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Matrix &Xi_sum) { + Eigen::Matrix &Xi_sum) { // useful identities Eigen::Matrix3d I_3x3 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d Z_3x1 = Eigen::MatrixXd::Zero(3,1); + Eigen::Vector3d Z_3x1 = Eigen::MatrixXd::Zero(3, 1); // now begin the integration double w_norm = w_hat.norm(); double d_th = w_norm * d_t; Eigen::Vector3d k_hat; - if(w_norm < 1e-8) { k_hat << Z_3x1;} - else { k_hat << w_hat / w_norm;} + if (w_norm < 1e-8) { + k_hat << Z_3x1; + } else { + k_hat << w_hat / w_norm; + } // comupute usefull identities - double d_t2 = std::pow(d_t,2); - double d_t3 = std::pow(d_t,3);; - double w_norm2 = std::pow(w_norm,2); - double w_norm3 = std::pow(w_norm,3); + double d_t2 = std::pow(d_t, 2); + double d_t3 = std::pow(d_t, 3); + ; + double w_norm2 = std::pow(w_norm, 2); + double w_norm3 = std::pow(w_norm, 3); double cos_dth = cos(d_th); double sin_dth = sin(d_th); - double d_th2 = std::pow(d_th,2); - double d_th3 = std::pow(d_th,3); + double d_th2 = std::pow(d_th, 2); + double d_th3 = std::pow(d_th, 3); Eigen::Matrix3d sK = ov_core::skew_x(k_hat); Eigen::Matrix3d sK2 = sK * sK; Eigen::Matrix3d sA = ov_core::skew_x(a_hat); // based on the delta theta, let's decide which integration will be used - bool small_w = (w_norm < 1.0/180 * M_PI / 2); - bool small_th = (d_th < 1.0/180 * M_PI / 2); + bool small_w = (w_norm < 1.0 / 180 * M_PI / 2); + bool small_th = (d_th < 1.0 / 180 * M_PI / 2); // integration components will be used later Eigen::Matrix3d R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; // now the R and J can be computed R_k1tok = I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2; - if(!small_th){ - Jr_k1tok = I_3x3 - (1.0-cos_dth) / d_th * sK + (d_th - sin_dth) / d_th * sK2; - }else{ + if (!small_th) { + Jr_k1tok = I_3x3 - (1.0 - cos_dth) / d_th * sK + (d_th - sin_dth) / d_th * sK2; + } else { Jr_k1tok = I_3x3 - sin_dth * sK + (1.0 - cos_dth) * sK2; } // now begin the integration - if(!small_w){ + if (!small_w) { // first order rotation integration with constant omega - Xi_1 = I_3x3 * d_t - + (1.0 - cos_dth) / w_norm * sK - + (d_t - sin_dth / w_norm) * sK2; + Xi_1 = I_3x3 * d_t + (1.0 - cos_dth) / w_norm * sK + (d_t - sin_dth / w_norm) * sK2; // second order rotation integration with constat omega - Xi_2 = 1.0/2 * d_t2 * I_3x3 - + (d_th - sin_dth) / w_norm2 * sK - + (1.0/2 * d_t2 - (1.0-cos_dth) / w_norm2) * sK2; + Xi_2 = 1.0 / 2 * d_t2 * I_3x3 + (d_th - sin_dth) / w_norm2 * sK + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sK2; // first order RAJ integratioin with constant omega and constant acc - Xi_3 = 1.0/2 * d_t2 * sA - + (sin_dth - d_th) / w_norm2 * sA * sK - + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA - + (1.0/2*d_t2 - (1.0-cos_dth) / w_norm2) * sA * sK2 - + (1.0/2*d_t2 - (1.0-cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - - (3*sin_dth - 2*d_th- d_th*cos_dth) / w_norm2 * k_hat.dot(a_hat)*sK2; + Xi_3 = 1.0 / 2 * d_t2 * sA + (sin_dth - d_th) / w_norm2 * sA * sK + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA + + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sA * sK2 + + (1.0 / 2 * d_t2 - (1.0 - cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - + (3 * sin_dth - 2 * d_th - d_th * cos_dth) / w_norm2 * k_hat.dot(a_hat) * sK2; // second order RAJ integration with constant omega and constant acc - Xi_4 = 1.0/6 * d_t3 * sA - + (2*(1.0-cos_dth) - d_th2) / (2*w_norm3) * sA * sK - + ((2*(1.0-cos_dth) - d_th * sin_dth)/w_norm3) * sK * sA - + ((sin_dth - d_th)/w_norm3 + d_t3/6) * sA *sK2 - + ((d_th - 2*sin_dth + 1.0/6*d_th3+d_th*cos_dth)/w_norm3) * (sK2 * sA + k_hat.dot(a_hat) * sK) - + (4*cos_dth -4 + d_th2 + d_th*sin_dth)/w_norm3 * k_hat.dot(a_hat) * sK2; + Xi_4 = 1.0 / 6 * d_t3 * sA + (2 * (1.0 - cos_dth) - d_th2) / (2 * w_norm3) * sA * sK + + ((2 * (1.0 - cos_dth) - d_th * sin_dth) / w_norm3) * sK * sA + ((sin_dth - d_th) / w_norm3 + d_t3 / 6) * sA * sK2 + + ((d_th - 2 * sin_dth + 1.0 / 6 * d_th3 + d_th * cos_dth) / w_norm3) * (sK2 * sA + k_hat.dot(a_hat) * sK) + + (4 * cos_dth - 4 + d_th2 + d_th * sin_dth) / w_norm3 * k_hat.dot(a_hat) * sK2; - }else { + } else { // first order rotation Xi_1 = d_t * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); // second order rotation Xi_2 = 1.0 / 2 * d_t * Xi_1; - //iint_R = 1.0/2 * d_t2 * (I_3x3 + sin_dth * sK + (1.0-cos_dth) * sK2); + // iint_R = 1.0/2 * d_t2 * (I_3x3 + sin_dth * sK + (1.0-cos_dth) * sK2); // first order RAJ - Xi_3 = 1.0 / 2 * d_t2 * (sA - + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) - + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); + Xi_3 = 1.0 / 2 * d_t2 * + (sA + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); // second order RAJ Xi_4 = 1.0 / 3 * d_t * Xi_3; - } // store the integrated parameters Xi_sum << R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; - // we are good to go return; } - - Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected) { - Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3,3); - Eigen::Vector3d e_1 = I_3x3.block<3,1>(0,0); - Eigen::Vector3d e_2 = I_3x3.block<3,1>(0,1); - Eigen::Vector3d e_3 = I_3x3.block<3,1>(0,2); + Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); + Eigen::Vector3d e_1 = I_3x3.block<3, 1>(0, 0); + Eigen::Vector3d e_2 = I_3x3.block<3, 1>(0, 1); + Eigen::Vector3d e_3 = I_3x3.block<3, 1>(0, 2); double w_1 = w_uncorrected(0); double w_2 = w_uncorrected(1); @@ -1004,14 +981,14 @@ Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eig // intrinsic parameters Eigen::MatrixXd H_Dw; - if(state->_options.do_calib_imu_intrinsics) { - H_Dw = Eigen::MatrixXd::Zero(3,6); - if(state->_options.imu_model == 0){ + if (state->_options.do_calib_imu_intrinsics) { + H_Dw = Eigen::MatrixXd::Zero(3, 6); + if (state->_options.imu_model == 0) { // Kalibr model - H_Dw << w_1*I_3x3, w_2*e_2, w_2*e_3, w_3*e_3; - }else{ + H_Dw << w_1 * I_3x3, w_2 * e_2, w_2 * e_3, w_3 * e_3; + } else { // RPNG model - H_Dw << w_1*e_1, w_2*e_1, w_2*e_2, w_3*I_3x3; + H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * I_3x3; } } @@ -1021,10 +998,10 @@ Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eig Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected) { - Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3,3); - Eigen::Vector3d e_1 = I_3x3.block<3,1>(0,0); - Eigen::Vector3d e_2 = I_3x3.block<3,1>(0,1); - Eigen::Vector3d e_3 = I_3x3.block<3,1>(0,2); + Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); + Eigen::Vector3d e_1 = I_3x3.block<3, 1>(0, 0); + Eigen::Vector3d e_2 = I_3x3.block<3, 1>(0, 1); + Eigen::Vector3d e_3 = I_3x3.block<3, 1>(0, 2); double a_1 = a_uncorrected(0); double a_2 = a_uncorrected(1); @@ -1032,38 +1009,35 @@ Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eig // intrinsic parameters Eigen::MatrixXd H_Da; - if(state->_options.do_calib_imu_intrinsics) { - H_Da = Eigen::MatrixXd::Zero(3,6); - if(state->_options.imu_model == 0){ + if (state->_options.do_calib_imu_intrinsics) { + H_Da = Eigen::MatrixXd::Zero(3, 6); + if (state->_options.imu_model == 0) { // kalibr model - H_Da << a_1*I_3x3, a_2*e_2, a_2*e_3, a_3*e_3; - }else{ + H_Da << a_1 * I_3x3, a_2 * e_2, a_2 * e_3, a_3 * e_3; + } else { // RPNG model - H_Da << a_1*e_1, a_2*e_1, a_2*e_2, a_3*I_3x3; + H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * I_3x3; } - } - // we are good to go return H_Da; } - Eigen::MatrixXd Propagator::compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI) { - Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3,3); - Eigen::Vector3d e_1 = I_3x3.block<3,1>(0,0); - Eigen::Vector3d e_2 = I_3x3.block<3,1>(0,1); + Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); + Eigen::Vector3d e_1 = I_3x3.block<3, 1>(0, 0); + Eigen::Vector3d e_2 = I_3x3.block<3, 1>(0, 1); double a_1 = a_inI(0); double a_2 = a_inI(1); double a_3 = a_inI(2); // intrinsic parameters - Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3,9); + Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3, 9); - if(state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity){ - H_Tg << a_1*I_3x3, a_2*I_3x3, a_3*I_3x3; + if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { + H_Tg << a_1 * I_3x3, a_2 * I_3x3, a_3 * I_3x3; } // we are good to go diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index d70e22994..64fe9f1e7 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -38,35 +38,56 @@ State::State(StateOptions &options) { current_id += _imu->size(); // Append the imu intrinsics to the state and covariance + // NOTE: these need to be right "next" to the IMU state in the covariance + // NOTE: since if calibrating these will evolve / be correlated during propagation _imu_x_dw = std::make_shared(6); _imu_x_da = std::make_shared(6); + if (options.imu_model == 0) { + // lower triangular of the matrix (column-wise) + Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); + _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; + _imu_x_dw->set_value(_imu_default); + _imu_x_dw->set_fej(_imu_default); + _imu_x_da->set_value(_imu_default); + _imu_x_da->set_fej(_imu_default); + } else { + // upper triangular of the matrix (column-wise) + Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); + _imu_default << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0; + _imu_x_dw->set_fej(_imu_default); + _imu_x_dw->set_fej(_imu_default); + _imu_x_da->set_fej(_imu_default); + _imu_x_da->set_fej(_imu_default); + } _imu_x_tg = std::make_shared(9); _imu_quat_gyrotoI = std::make_shared(); _imu_quat_acctoI = std::make_shared(); + if (options.do_calib_imu_intrinsics) { - - if(options.do_calib_imu_intrinsics) { + // Gyroscope dw _imu_x_dw->set_local_id(current_id); _variables.push_back(_imu_x_dw); current_id += _imu_x_dw->size(); + // Accelerometer da _imu_x_da->set_local_id(current_id); _variables.push_back(_imu_x_da); current_id += _imu_x_da->size(); - if(options.do_calib_imu_g_sensitivity){ + // Gyroscope gravity sensitivity + if (options.do_calib_imu_g_sensitivity) { _imu_x_tg->set_local_id(current_id); _variables.push_back(_imu_x_tg); current_id += _imu_x_tg->size(); } - if(options.imu_model == 0){ - // if kalibr model, R_GyrotoI is calibrated + // If kalibr model, R_GyrotoI is calibrated + // If rpng model, R_AcctoI is calibrated + if (options.imu_model == 0) { _imu_quat_gyrotoI->set_local_id(current_id); _variables.push_back(_imu_quat_gyrotoI); current_id += _imu_quat_gyrotoI->size(); - }else{ - // if rpng model, R_AcctoI is calibrated + } else { _imu_quat_acctoI->set_local_id(current_id); _variables.push_back(_imu_quat_acctoI); current_id += _imu_quat_acctoI->size(); @@ -113,22 +134,21 @@ State::State(StateOptions &options) { _Cov = 1e-3 * Eigen::MatrixXd::Identity(current_id, current_id); // Finally, set some of our priors for our calibration parameters - if(_options.do_calib_imu_intrinsics){ - _Cov.block<6,6>(_imu_x_dw->id(),_imu_x_dw->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); - _Cov.block<6,6>(_imu_x_da->id(),_imu_x_da->id()) = std::pow(0.008,2) * Eigen::Matrix::Identity(); - if(_options.do_calib_imu_g_sensitivity){ - _Cov.block<9,9>(_imu_x_tg->id(),_imu_x_tg->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); + if (_options.do_calib_imu_intrinsics) { + _Cov.block<6, 6>(_imu_x_dw->id(), _imu_x_dw->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block<6, 6>(_imu_x_da->id(), _imu_x_da->id()) = std::pow(0.008, 2) * Eigen::Matrix::Identity(); + if (_options.do_calib_imu_g_sensitivity) { + _Cov.block<9, 9>(_imu_x_tg->id(), _imu_x_tg->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } - if(_options.imu_model == 0){ + if (_options.imu_model == 0) { // if kalibr model, R_GyrotoI is calibrated - _Cov.block<3,3>(_imu_quat_gyrotoI->id(), _imu_quat_gyrotoI->id()) = std::pow(0.005,2) * Eigen::Matrix::Identity(); - }else{ + _Cov.block<3, 3>(_imu_quat_gyrotoI->id(), _imu_quat_gyrotoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + } else { // if rpng model, R_AcctoI is calibrated - _Cov.block<3,3>(_imu_quat_acctoI->id(),_imu_quat_acctoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block<3, 3>(_imu_quat_acctoI->id(), _imu_quat_acctoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } } - if (_options.do_calib_camera_timeoffset) { _Cov(_calib_dt_CAMtoIMU->id(), _calib_dt_CAMtoIMU->id()) = std::pow(0.01, 2); } @@ -148,55 +168,42 @@ State::State(StateOptions &options) { } } - Eigen::Matrix3d State::Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if(_options.imu_model == 0){ + if (_options.imu_model == 0) { // if kalibr model, lower triangular of the matrix is used - Dw << _imu_x_dw->value()(0), 0, 0, - _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, - _imu_x_dw->value()(2), _imu_x_dw->value()(4), _imu_x_dw->value()(5); - }else{ + Dw << _imu_x_dw->value()(0), 0, 0, _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), + _imu_x_dw->value()(5); + } else { // if rpng model, upper triangular of the matrix is used - Dw << _imu_x_dw->value()(0), _imu_x_dw->value()(1), _imu_x_dw->value()(3), - 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), - 0, 0, _imu_x_dw->value()(5); + Dw << _imu_x_dw->value()(0), _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), 0, 0, + _imu_x_dw->value()(5); } return Dw; } - Eigen::Matrix3d State::Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if(_options.imu_model == 0){ + if (_options.imu_model == 0) { // if kalibr model, lower triangular of the matrix is used - Da << _imu_x_da->value()(0), 0, 0, - _imu_x_da->value()(1), _imu_x_da->value()(3), 0, - _imu_x_da->value()(2), _imu_x_da->value()(4), _imu_x_da->value()(5); - }else{ + Da << _imu_x_da->value()(0), 0, 0, _imu_x_da->value()(1), _imu_x_da->value()(3), 0, _imu_x_da->value()(2), _imu_x_da->value()(4), + _imu_x_da->value()(5); + } else { // if rpng model, upper triangular of the matrix is used - Da << _imu_x_da->value()(0), _imu_x_da->value()(1), _imu_x_da->value()(3), - 0, _imu_x_da->value()(2), _imu_x_da->value()(4), - 0, 0, _imu_x_da->value()(5); + Da << _imu_x_da->value()(0), _imu_x_da->value()(1), _imu_x_da->value()(3), 0, _imu_x_da->value()(2), _imu_x_da->value()(4), 0, 0, + _imu_x_da->value()(5); } return Da; } - Eigen::Matrix3d State::Tg() { Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << _imu_x_tg->value()(0), _imu_x_tg->value()(3), _imu_x_tg->value()(6), - _imu_x_tg->value()(1), _imu_x_tg->value()(4), _imu_x_tg->value()(7), - _imu_x_tg->value()(2), _imu_x_tg->value()(5), _imu_x_tg->value()(8); + Tg << _imu_x_tg->value()(0), _imu_x_tg->value()(3), _imu_x_tg->value()(6), _imu_x_tg->value()(1), _imu_x_tg->value()(4), + _imu_x_tg->value()(7), _imu_x_tg->value()(2), _imu_x_tg->value()(5), _imu_x_tg->value()(8); return Tg; } +Eigen::Matrix3d State::R_AcctoI() { return _imu_quat_acctoI->Rot(); } -Eigen::Matrix3d State::R_AcctoI() { - return _imu_quat_acctoI->Rot(); -} - -Eigen::Matrix3d State::R_GyrotoI() { - return _imu_quat_gyrotoI->Rot(); -} +Eigen::Matrix3d State::R_GyrotoI() { return _imu_quat_gyrotoI->Rot(); } diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index b7c62ce09..2998d0a38 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -110,6 +110,8 @@ struct StateOptions { parser->parse_config("max_msckf_in_update", max_msckf_in_update); parser->parse_config("num_aruco", max_aruco_features); parser->parse_config("max_cameras", num_cameras); + + // Feature representations std::string rep1 = ov_type::LandmarkRepresentation::as_string(feat_rep_msckf); parser->parse_config("feat_rep_msckf", rep1); feat_rep_msckf = ov_type::LandmarkRepresentation::from_string(rep1); @@ -123,12 +125,13 @@ struct StateOptions { // IMU model std::string imu_model_str = "kalibr"; parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); - if(imu_model_str == "kalibr") imu_model = 0; - else if(imu_model_str == "rpng") imu_model = 1; - else { - PRINT_ERROR(RED "StateOption(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); + if (imu_model_str == "kalibr" || imu_model_str == "calibrated") { + imu_model = 0; + } else if (imu_model_str == "rpng") { + imu_model = 1; + } else { + PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); } - } PRINT_DEBUG(" - use_fej: %d\n", do_fej); PRINT_DEBUG(" - use_imuavg: %d\n", imu_avg); From 3a1718a3e08556705d5cd3ac2b59cbfca1aec392 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 12:38:33 -0500 Subject: [PATCH 18/60] start of cleanup --- config/rpng_sim/kalibr_imu_chain.yaml | 2 +- ov_msckf/src/core/VioManager.cpp | 33 +++++++-- ov_msckf/src/core/VioManagerOptions.h | 19 ++++-- ov_msckf/src/ros/RosVisualizerHelper.h | 32 ++++----- ov_msckf/src/state/Propagator.cpp | 89 ++++++++++++------------ ov_msckf/src/state/State.cpp | 94 +++++++++++++------------- ov_msckf/src/state/State.h | 20 ++++-- ov_msckf/src/state/StateOptions.h | 12 +++- 8 files changed, 172 insertions(+), 129 deletions(-) diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 715a5a4ad..3d881e69f 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -13,7 +13,7 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "calibrated" # calibrated (same as kalibr), kalibr, rpng + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 80014a864..d1ef09447 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -48,11 +48,11 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { state = std::make_shared(params.state_options); // Set the IMU intrinsics - state->_imu_x_dw->set_value(params.imu_config.imu_x_dw); - state->_imu_x_da->set_value(params.imu_config.imu_x_da); - state->_imu_x_tg->set_value(params.imu_config.imu_x_tg); - state->_imu_quat_gyrotoI->set_value(params.imu_config.imu_quat_GyrotoI); - state->_imu_quat_acctoI->set_value(params.imu_config.imu_quat_AcctoI); + state->_calib_imu_dw->set_value(params.imu_config.imu_x_dw); + state->_calib_imu_da->set_value(params.imu_config.imu_x_da); + state->_calib_imu_tg->set_value(params.imu_config.imu_x_tg); + state->_calib_imu_GYROtoIMU->set_value(params.imu_config.imu_quat_GyrotoI); + state->_calib_imu_ACCtoIMU->set_value(params.imu_config.imu_quat_AcctoI); // Timeoffset from camera to IMU Eigen::VectorXd temp_camimu_dt; @@ -720,6 +720,29 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) calib->quat()(3), calib->pos()(0), calib->pos()(1), calib->pos()(2)); } } + + // Debug for imu intrinsics + if (state->_options.do_calib_imu_g_sensitivity) { + PRINT_INFO("Tg = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_tg->value()(0), state->_calib_imu_tg->value()(1), + state->_calib_imu_tg->value()(2), state->_calib_imu_tg->value()(3), state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), + state->_calib_imu_tg->value()(6), state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); + } + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { + PRINT_INFO("Dw = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), + state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); + PRINT_INFO("Da = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), + state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); + PRINT_INFO("q_GYROtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_GYROtoIMU->value()(0), state->_calib_imu_GYROtoIMU->value()(1), + state->_calib_imu_GYROtoIMU->value()(2), state->_calib_imu_GYROtoIMU->value()(3)); + } + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { + PRINT_INFO("Dw = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), + state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); + PRINT_INFO("Da = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), + state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); + PRINT_INFO("q_ACCtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_ACCtoIMU->value()(0), state->_calib_imu_ACCtoIMU->value()(1), + state->_calib_imu_ACCtoIMU->value()(2), state->_calib_imu_ACCtoIMU->value()(3)); + } } bool VioManager::try_to_initialize() { diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 55873e77e..0c7b0cd98 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -292,6 +292,7 @@ struct VioManagerOptions { imu_config.imu_model = 1; } else { PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); + std::exit(EXIT_FAILURE); } // IMU intrinsics @@ -306,22 +307,19 @@ struct VioManagerOptions { Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); parser->parse_external("relative_config_imu", "imu0", "Tg", Tg); - // generate the parameters we need + // Generate the parameters we need // TODO: error here if this returns a NaN value (i.e. invalid matrix specified) Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d R_AcctoI = R_ItoAcc.transpose(); Eigen::Matrix3d R_GyrotoI = R_ItoGyro.transpose(); - // store these parameters + // kalibr model: lower triangular of the matrix and R_GYROtoI + // rpng model: upper triangular of the matrix and R_ACCtoI if (imu_config.imu_model == 0) { - // kalibr model - // lower triangular of the matrix imu_config.imu_x_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); imu_config.imu_x_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); } else { - // rpng model - // upper triangular of the matrix imu_config.imu_x_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2); imu_config.imu_x_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); } @@ -342,6 +340,7 @@ struct VioManagerOptions { std::exit(EXIT_FAILURE); } PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); + PRINT_DEBUG("CAMERA PARAMETERS:\n"); for (int n = 0; n < state_options.num_cameras; n++) { std::stringstream ss; ss << "cam_" << n << "_fisheye:" << (std::dynamic_pointer_cast(camera_intrinsics.at(n)) != nullptr) << std::endl; @@ -358,6 +357,14 @@ struct VioManagerOptions { ss << "T_C" << n << "toI:" << std::endl << T_CtoI << std::endl << std::endl; PRINT_DEBUG(ss.str().c_str()); } + PRINT_DEBUG("IMU PARAMETERS:\n"); + std::stringstream ss; + ss << "Dw (columnwise):" << std::endl << imu_config.imu_x_dw.transpose() << std::endl; + ss << "Da (columnwise):" << std::endl << imu_config.imu_x_da.transpose() << std::endl; + ss << "Tg (columnwise):" << std::endl << imu_config.imu_x_tg.transpose() << std::endl; + ss << "R_GYROtoI:" << std::endl << imu_config.imu_quat_GyrotoI << std::endl; + ss << "R_ACCtoI:" << std::endl << imu_config.imu_quat_AcctoI << std::endl; + PRINT_DEBUG(ss.str().c_str()); } // TRACKERS =============================== diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index d82d46765..5fa71090f 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -406,10 +406,10 @@ class RosVisualizerHelper { of_state_std.precision(8); // imu intrinsics: dw - of_state_est << state->_imu_x_dw->value()(0) << " " << state->_imu_x_dw->value()(1) << " " << state->_imu_x_dw->value()(2) << " " - << state->_imu_x_dw->value()(3) << " " << state->_imu_x_dw->value()(4) << " " << state->_imu_x_dw->value()(5) << " "; + of_state_est << state->_calib_imu_dw->value()(0) << " " << state->_calib_imu_dw->value()(1) << " " << state->_calib_imu_dw->value()(2) << " " + << state->_calib_imu_dw->value()(3) << " " << state->_calib_imu_dw->value()(4) << " " << state->_calib_imu_dw->value()(5) << " "; if (state->_options.do_calib_imu_intrinsics) { - int index_dw = state->_imu_x_dw->id(); + int index_dw = state->_calib_imu_dw->id(); of_state_std << std::sqrt(cov(index_dw + 0, index_dw + 0)) << " " << std::sqrt(cov(index_dw + 1, index_dw + 1)) << " " << std::sqrt(cov(index_dw + 2, index_dw + 2)) << " " << std::sqrt(cov(index_dw + 3, index_dw + 3)) << " " << std::sqrt(cov(index_dw + 4, index_dw + 4)) << " " << std::sqrt(cov(index_dw + 5, index_dw + 5)) << " "; @@ -419,10 +419,10 @@ class RosVisualizerHelper { } // imu intrinsics: da - of_state_est << state->_imu_x_da->value()(0) << " " << state->_imu_x_da->value()(1) << " " << state->_imu_x_da->value()(2) << " " - << state->_imu_x_da->value()(3) << " " << state->_imu_x_da->value()(4) << " " << state->_imu_x_da->value()(5) << " "; + of_state_est << state->_calib_imu_da->value()(0) << " " << state->_calib_imu_da->value()(1) << " " << state->_calib_imu_da->value()(2) << " " + << state->_calib_imu_da->value()(3) << " " << state->_calib_imu_da->value()(4) << " " << state->_calib_imu_da->value()(5) << " "; if (state->_options.do_calib_imu_intrinsics) { - int index_da = state->_imu_x_da->id(); + int index_da = state->_calib_imu_da->id(); of_state_std << std::sqrt(cov(index_da + 0, index_da + 0)) << " " << std::sqrt(cov(index_da + 1, index_da + 1)) << " " << std::sqrt(cov(index_da + 2, index_da + 2)) << " " << std::sqrt(cov(index_da + 3, index_da + 3)) << " " << std::sqrt(cov(index_da + 4, index_da + 4)) << " " << std::sqrt(cov(index_da + 5, index_da + 5)) << " "; @@ -432,11 +432,11 @@ class RosVisualizerHelper { } // imu intrinsics: tg - of_state_est << state->_imu_x_tg->value()(0) << " " << state->_imu_x_tg->value()(1) << " " << state->_imu_x_tg->value()(2) << " " - << state->_imu_x_tg->value()(3) << " " << state->_imu_x_tg->value()(4) << " " << state->_imu_x_tg->value()(5) << " " - << state->_imu_x_tg->value()(6) << " " << state->_imu_x_tg->value()(7) << " " << state->_imu_x_tg->value()(8) << " "; + of_state_est << state->_calib_imu_tg->value()(0) << " " << state->_calib_imu_tg->value()(1) << " " << state->_calib_imu_tg->value()(2) << " " + << state->_calib_imu_tg->value()(3) << " " << state->_calib_imu_tg->value()(4) << " " << state->_calib_imu_tg->value()(5) << " " + << state->_calib_imu_tg->value()(6) << " " << state->_calib_imu_tg->value()(7) << " " << state->_calib_imu_tg->value()(8) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { - int index_tg = state->_imu_x_tg->id(); + int index_tg = state->_calib_imu_tg->id(); of_state_std << std::sqrt(cov(index_tg + 0, index_tg + 0)) << " " << std::sqrt(cov(index_tg + 1, index_tg + 1)) << " " << std::sqrt(cov(index_tg + 2, index_tg + 2)) << " " << std::sqrt(cov(index_tg + 3, index_tg + 3)) << " " << std::sqrt(cov(index_tg + 4, index_tg + 4)) << " " << std::sqrt(cov(index_tg + 5, index_tg + 5)) << " " @@ -449,10 +449,10 @@ class RosVisualizerHelper { } // imu intrinsics: kalibr R_gyrotoI - of_state_est << state->_imu_quat_gyrotoI->value()(0) << " " << state->_imu_quat_gyrotoI->value()(1) << " " - << state->_imu_quat_gyrotoI->value()(2) << " " << state->_imu_quat_gyrotoI->value()(3) << " "; + of_state_est << state->_calib_imu_GYROtoIMU->value()(0) << " " << state->_calib_imu_GYROtoIMU->value()(1) << " " + << state->_calib_imu_GYROtoIMU->value()(2) << " " << state->_calib_imu_GYROtoIMU->value()(3) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { - int index_wtoI = state->_imu_quat_gyrotoI->id(); + int index_wtoI = state->_calib_imu_GYROtoIMU->id(); of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " " << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " "; } else { @@ -460,10 +460,10 @@ class RosVisualizerHelper { } // imu intrinsics: rpng R_acctoI - of_state_est << state->_imu_quat_acctoI->value()(0) << " " << state->_imu_quat_acctoI->value()(1) << " " - << state->_imu_quat_acctoI->value()(2) << " " << state->_imu_quat_acctoI->value()(3) << " "; + of_state_est << state->_calib_imu_ACCtoIMU->value()(0) << " " << state->_calib_imu_ACCtoIMU->value()(1) << " " + << state->_calib_imu_ACCtoIMU->value()(2) << " " << state->_calib_imu_ACCtoIMU->value()(3) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { - int index_atoI = state->_imu_quat_acctoI->id(); + int index_atoI = state->_calib_imu_ACCtoIMU->id(); of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " " << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " "; } else { diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index b62a8afaa..0e0e82f79 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -90,36 +90,31 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest } } - // // Last angular velocity (used for cloning when estimating time offset) - // Eigen::Matrix last_w = Eigen::Matrix::Zero(); - // if (prop_data.size() > 1) - // last_w = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); - // else if (!prop_data.empty()) - // last_w = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); - // Last angular velocity (used for cloning when estimating time offset) // Remember to correct them before we store them - Eigen::Matrix last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); - Eigen::Matrix last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; - Eigen::Matrix last_w = - state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); + Eigen::Vector3d last_a = Eigen::Vector3d::Zero(); + Eigen::Vector3d last_w = Eigen::Vector3d::Zero(); + if (!prop_data.empty()) { + last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); + Eigen::Vector3d last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; + last_w = state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); + } // Do the update to the covariance with our "summed" state transition and IMU noise addition... std::vector> Phi_order; Phi_order.push_back(state->_imu); if (state->_options.do_calib_imu_intrinsics) { - Phi_order.push_back(state->_imu_x_dw); - Phi_order.push_back(state->_imu_x_da); + Phi_order.push_back(state->_calib_imu_dw); + Phi_order.push_back(state->_calib_imu_da); if (state->_options.do_calib_imu_g_sensitivity) { - Phi_order.push_back(state->_imu_x_tg); + Phi_order.push_back(state->_calib_imu_tg); } if (state->_options.imu_model == 0) { - Phi_order.push_back(state->_imu_quat_gyrotoI); + Phi_order.push_back(state->_calib_imu_GYROtoIMU); } else { - Phi_order.push_back(state->_imu_quat_acctoI); + Phi_order.push_back(state->_calib_imu_ACCtoIMU); } } - StateHelper::EKFPropagation(state, Phi_order, Phi_order, Phi_summed, Qd_summed); // Set timestamp data @@ -475,21 +470,21 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d int th_wtoI_id = -1; if (state->_options.do_calib_imu_intrinsics) { Dw_id = local_size; - local_size += state->_imu_x_dw->size(); + local_size += state->_calib_imu_dw->size(); Da_id = local_size; - local_size += state->_imu_x_da->size(); + local_size += state->_calib_imu_da->size(); if (state->_options.do_calib_imu_g_sensitivity) { Tg_id = local_size; - local_size += state->_imu_x_tg->size(); + local_size += state->_calib_imu_tg->size(); } if (state->_options.imu_model == 0) { // Kalibr model th_wtoI_id = local_size; - local_size += state->_imu_quat_gyrotoI->size(); + local_size += state->_calib_imu_GYROtoIMU->size(); } else { // RPNG model th_atoI_id = local_size; - local_size += state->_imu_quat_acctoI->size(); + local_size += state->_calib_imu_ACCtoIMU->size(); } } @@ -549,28 +544,28 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // begin to add the state transition matrix for the omega intrinsics part if (Dw_id != -1) { - F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); - F.block(p_id, Dw_id, 3, state->_imu_x_dw->size()) = -R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state, w_uncorrected); - F.block(v_id, Dw_id, 3, state->_imu_x_dw->size()) = -R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state, w_uncorrected); - F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); + F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); + F.block(p_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state, w_uncorrected); + F.block(v_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state, w_uncorrected); + F.block(Dw_id, Dw_id, state->_calib_imu_dw->size(), state->_calib_imu_dw->size()).setIdentity(); } // begin to add the state transition matrix for the acc intrinsics part if (Da_id != -1) { - F.block(Da_id, Da_id, state->_imu_x_da->size(), state->_imu_x_da->size()).setIdentity(); - F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * compute_H_Da(state, w_uncorrected); - F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = + F.block(Da_id, Da_id, state->_calib_imu_da->size(), state->_calib_imu_da->size()).setIdentity(); + F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * compute_H_Da(state, w_uncorrected); + F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = + F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * compute_H_Da(state, a_uncorrected); } // add the state trasition matrix of the tg part if (Tg_id != -1) { - F.block(Tg_id, Tg_id, state->_imu_x_tg->size(), state->_imu_x_tg->size()).setIdentity(); - F.block(th_id, Tg_id, 3, state->_imu_x_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); - F.block(p_id, Tg_id, 3, state->_imu_x_tg->size()) = R_k.transpose() * Xi_4 * R_wtoI * Dw * compute_H_Tg(state, a_k); - F.block(v_id, Tg_id, 3, state->_imu_x_tg->size()) = R_k.transpose() * Xi_3 * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); + F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(p_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_4 * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(v_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_3 * R_wtoI * Dw * compute_H_Tg(state, a_k); } // begin to add the state transition matrix for the acctoI part @@ -629,21 +624,21 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d int th_wtoI_id = -1; if (state->_options.do_calib_imu_intrinsics) { Dw_id = local_size; - local_size += state->_imu_x_dw->size(); + local_size += state->_calib_imu_dw->size(); Da_id = local_size; - local_size += state->_imu_x_da->size(); + local_size += state->_calib_imu_da->size(); if (state->_options.do_calib_imu_g_sensitivity) { Tg_id = local_size; - local_size += state->_imu_x_tg->size(); + local_size += state->_calib_imu_tg->size(); } if (state->_options.imu_model == 0) { // Kalibr model th_wtoI_id = local_size; - local_size += state->_imu_quat_gyrotoI->size(); + local_size += state->_calib_imu_GYROtoIMU->size(); } else { // RPNG model th_atoI_id = local_size; - local_size += state->_imu_quat_acctoI->size(); + local_size += state->_calib_imu_ACCtoIMU->size(); } } @@ -698,16 +693,16 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the omega intrinsics part if (Dw_id != -1) { - F.block(Dw_id, Dw_id, state->_imu_x_dw->size(), state->_imu_x_dw->size()).setIdentity(); - F.block(th_id, Dw_id, 3, state->_imu_x_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); + F.block(Dw_id, Dw_id, state->_calib_imu_dw->size(), state->_calib_imu_dw->size()).setIdentity(); + F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); } // begin to add the state transition matrix for the acc intrinsics part if (Da_id != -1) { - F.block(th_id, Da_id, 3, state->_imu_x_da->size()) = -Jr * dt * R_wtoI * Tg * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(p_id, Da_id, 3, state->_imu_x_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(v_id, Da_id, 3, state->_imu_x_da->size()) = R_k.transpose() * dt * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(Da_id, Da_id, state->_imu_x_da->size(), state->_imu_x_da->size()).setIdentity(); + F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Tg * R_atoI * compute_H_Da(state, a_uncorrected); + F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * compute_H_Da(state, a_uncorrected); + F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * dt * R_atoI * compute_H_Da(state, a_uncorrected); + F.block(Da_id, Da_id, state->_calib_imu_da->size(), state->_calib_imu_da->size()).setIdentity(); } // begin to add the state transition matrix for the acc intrinsics part @@ -726,8 +721,8 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the gravity sensitivity part if (Tg_id != -1) { - F.block(Tg_id, Tg_id, state->_imu_x_tg->size(), state->_imu_x_tg->size()).setIdentity(); - F.block(th_id, Tg_id, 3, state->_imu_x_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); + F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); } // Noise jacobian diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index 64fe9f1e7..e9b02d7d5 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -40,57 +40,57 @@ State::State(StateOptions &options) { // Append the imu intrinsics to the state and covariance // NOTE: these need to be right "next" to the IMU state in the covariance // NOTE: since if calibrating these will evolve / be correlated during propagation - _imu_x_dw = std::make_shared(6); - _imu_x_da = std::make_shared(6); + _calib_imu_dw = std::make_shared(6); + _calib_imu_da = std::make_shared(6); if (options.imu_model == 0) { // lower triangular of the matrix (column-wise) Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; - _imu_x_dw->set_value(_imu_default); - _imu_x_dw->set_fej(_imu_default); - _imu_x_da->set_value(_imu_default); - _imu_x_da->set_fej(_imu_default); + _calib_imu_dw->set_value(_imu_default); + _calib_imu_dw->set_fej(_imu_default); + _calib_imu_da->set_value(_imu_default); + _calib_imu_da->set_fej(_imu_default); } else { // upper triangular of the matrix (column-wise) Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); _imu_default << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0; - _imu_x_dw->set_fej(_imu_default); - _imu_x_dw->set_fej(_imu_default); - _imu_x_da->set_fej(_imu_default); - _imu_x_da->set_fej(_imu_default); + _calib_imu_dw->set_fej(_imu_default); + _calib_imu_dw->set_fej(_imu_default); + _calib_imu_da->set_fej(_imu_default); + _calib_imu_da->set_fej(_imu_default); } - _imu_x_tg = std::make_shared(9); - _imu_quat_gyrotoI = std::make_shared(); - _imu_quat_acctoI = std::make_shared(); + _calib_imu_tg = std::make_shared(9); + _calib_imu_GYROtoIMU = std::make_shared(); + _calib_imu_ACCtoIMU = std::make_shared(); if (options.do_calib_imu_intrinsics) { // Gyroscope dw - _imu_x_dw->set_local_id(current_id); - _variables.push_back(_imu_x_dw); - current_id += _imu_x_dw->size(); + _calib_imu_dw->set_local_id(current_id); + _variables.push_back(_calib_imu_dw); + current_id += _calib_imu_dw->size(); // Accelerometer da - _imu_x_da->set_local_id(current_id); - _variables.push_back(_imu_x_da); - current_id += _imu_x_da->size(); + _calib_imu_da->set_local_id(current_id); + _variables.push_back(_calib_imu_da); + current_id += _calib_imu_da->size(); // Gyroscope gravity sensitivity if (options.do_calib_imu_g_sensitivity) { - _imu_x_tg->set_local_id(current_id); - _variables.push_back(_imu_x_tg); - current_id += _imu_x_tg->size(); + _calib_imu_tg->set_local_id(current_id); + _variables.push_back(_calib_imu_tg); + current_id += _calib_imu_tg->size(); } - // If kalibr model, R_GyrotoI is calibrated - // If rpng model, R_AcctoI is calibrated + // If kalibr model, R_GYROtoIMU is calibrated + // If rpng model, R_ACCtoIMU is calibrated if (options.imu_model == 0) { - _imu_quat_gyrotoI->set_local_id(current_id); - _variables.push_back(_imu_quat_gyrotoI); - current_id += _imu_quat_gyrotoI->size(); + _calib_imu_GYROtoIMU->set_local_id(current_id); + _variables.push_back(_calib_imu_GYROtoIMU); + current_id += _calib_imu_GYROtoIMU->size(); } else { - _imu_quat_acctoI->set_local_id(current_id); - _variables.push_back(_imu_quat_acctoI); - current_id += _imu_quat_acctoI->size(); + _calib_imu_ACCtoIMU->set_local_id(current_id); + _variables.push_back(_calib_imu_ACCtoIMU); + current_id += _calib_imu_ACCtoIMU->size(); } } @@ -135,17 +135,17 @@ State::State(StateOptions &options) { // Finally, set some of our priors for our calibration parameters if (_options.do_calib_imu_intrinsics) { - _Cov.block<6, 6>(_imu_x_dw->id(), _imu_x_dw->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); - _Cov.block<6, 6>(_imu_x_da->id(), _imu_x_da->id()) = std::pow(0.008, 2) * Eigen::Matrix::Identity(); + _Cov.block<6, 6>(_calib_imu_dw->id(), _calib_imu_dw->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block<6, 6>(_calib_imu_da->id(), _calib_imu_da->id()) = std::pow(0.008, 2) * Eigen::Matrix::Identity(); if (_options.do_calib_imu_g_sensitivity) { - _Cov.block<9, 9>(_imu_x_tg->id(), _imu_x_tg->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block<9, 9>(_calib_imu_tg->id(), _calib_imu_tg->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } if (_options.imu_model == 0) { // if kalibr model, R_GyrotoI is calibrated - _Cov.block<3, 3>(_imu_quat_gyrotoI->id(), _imu_quat_gyrotoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block<3, 3>(_calib_imu_GYROtoIMU->id(), _calib_imu_GYROtoIMU->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } else { // if rpng model, R_AcctoI is calibrated - _Cov.block<3, 3>(_imu_quat_acctoI->id(), _imu_quat_acctoI->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block<3, 3>(_calib_imu_ACCtoIMU->id(), _calib_imu_ACCtoIMU->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } } @@ -172,12 +172,12 @@ Eigen::Matrix3d State::Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); if (_options.imu_model == 0) { // if kalibr model, lower triangular of the matrix is used - Dw << _imu_x_dw->value()(0), 0, 0, _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), - _imu_x_dw->value()(5); + Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), + _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); } else { // if rpng model, upper triangular of the matrix is used - Dw << _imu_x_dw->value()(0), _imu_x_dw->value()(1), _imu_x_dw->value()(3), 0, _imu_x_dw->value()(2), _imu_x_dw->value()(4), 0, 0, - _imu_x_dw->value()(5); + Dw << _calib_imu_dw->value()(0), _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), + _calib_imu_dw->value()(4), 0, 0, _calib_imu_dw->value()(5); } return Dw; } @@ -186,24 +186,24 @@ Eigen::Matrix3d State::Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); if (_options.imu_model == 0) { // if kalibr model, lower triangular of the matrix is used - Da << _imu_x_da->value()(0), 0, 0, _imu_x_da->value()(1), _imu_x_da->value()(3), 0, _imu_x_da->value()(2), _imu_x_da->value()(4), - _imu_x_da->value()(5); + Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), + _calib_imu_da->value()(4), _calib_imu_da->value()(5); } else { // if rpng model, upper triangular of the matrix is used - Da << _imu_x_da->value()(0), _imu_x_da->value()(1), _imu_x_da->value()(3), 0, _imu_x_da->value()(2), _imu_x_da->value()(4), 0, 0, - _imu_x_da->value()(5); + Da << _calib_imu_da->value()(0), _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), + _calib_imu_da->value()(4), 0, 0, _calib_imu_da->value()(5); } return Da; } Eigen::Matrix3d State::Tg() { Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << _imu_x_tg->value()(0), _imu_x_tg->value()(3), _imu_x_tg->value()(6), _imu_x_tg->value()(1), _imu_x_tg->value()(4), - _imu_x_tg->value()(7), _imu_x_tg->value()(2), _imu_x_tg->value()(5), _imu_x_tg->value()(8); + Tg << _calib_imu_tg->value()(0), _calib_imu_tg->value()(3), _calib_imu_tg->value()(6), _calib_imu_tg->value()(1), + _calib_imu_tg->value()(4), _calib_imu_tg->value()(7), _calib_imu_tg->value()(2), _calib_imu_tg->value()(5), _calib_imu_tg->value()(8); return Tg; } -Eigen::Matrix3d State::R_AcctoI() { return _imu_quat_acctoI->Rot(); } +Eigen::Matrix3d State::R_AcctoI() { return _calib_imu_ACCtoIMU->Rot(); } -Eigen::Matrix3d State::R_GyrotoI() { return _imu_quat_gyrotoI->Rot(); } +Eigen::Matrix3d State::R_GyrotoI() { return _calib_imu_GYROtoIMU->Rot(); } diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 4dd925fd7..41d64622c 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -128,12 +128,20 @@ class State { /// Camera intrinsics camera objects std::unordered_map> _cam_intrinsics_cameras; - /// IMU intrinsics - std::shared_ptr _imu_x_dw; - std::shared_ptr _imu_x_da; - std::shared_ptr _imu_quat_acctoI; - std::shared_ptr _imu_quat_gyrotoI; - std::shared_ptr _imu_x_tg; + /// Gyroscope IMU intrinsics (scale imperfection and axis misalignment) + std::shared_ptr _calib_imu_dw; + + /// Accelerometer IMU intrinsics (scale imperfection and axis misalignment) + std::shared_ptr _calib_imu_da; + + /// Rotation from gyroscope frame to the "IMU" accelerometer frame + std::shared_ptr _calib_imu_GYROtoIMU; + + /// Rotation from accelerometer to the "IMU" gyroscope frame frame + std::shared_ptr _calib_imu_ACCtoIMU; + + /// Gyroscope gravity sensitivity + std::shared_ptr _calib_imu_tg; private: // Define that the state helper is a friend class of this class diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 2998d0a38..6fc891c60 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -99,11 +99,15 @@ struct StateOptions { parser->parse_config("use_imuavg", imu_avg); parser->parse_config("use_rk4int", use_rk4_integration); parser->parse_config("use_analytic_int", use_analytic_integration); + + // Calibration booleans parser->parse_config("calib_cam_extrinsics", do_calib_camera_pose); parser->parse_config("calib_cam_intrinsics", do_calib_camera_intrinsics); parser->parse_config("calib_cam_timeoffset", do_calib_camera_timeoffset); parser->parse_config("calib_imu_intrinsics", do_calib_imu_intrinsics); parser->parse_config("calib_imu_g_sensitivity", do_calib_imu_g_sensitivity); + + // State parameters parser->parse_config("max_clones", max_clone_size); parser->parse_config("max_slam", max_slam_features); parser->parse_config("max_slam_in_update", max_slam_in_update); @@ -130,7 +134,13 @@ struct StateOptions { } else if (imu_model_str == "rpng") { imu_model = 1; } else { - PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); + PRINT_ERROR(RED "invalid imu model: %s\n" RESET, imu_model_str.c_str()); + std::exit(EXIT_FAILURE); + } + if (imu_model_str == "calibrated" && (do_calib_imu_intrinsics || do_calib_imu_g_sensitivity)) { + PRINT_ERROR(RED "calibrated IMU model selected, but requested calibration!\n" RESET); + PRINT_ERROR(RED "please select what model you have: kalibr, rpng\n" RESET); + std::exit(EXIT_FAILURE); } } PRINT_DEBUG(" - use_fej: %d\n", do_fej); From aa5a0afd405316124c58b0b07fbdfcb43cacc82a Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 13:56:48 -0500 Subject: [PATCH 19/60] more cleanup --- config/euroc_mav/estimator_config.yaml | 2 +- config/kaist/estimator_config.yaml | 2 +- config/kaist/pgeneva_ros_kaist.launch.old | 2 +- .../kaist_vio/pgeneva_ros_kaistvio.launch.old | 2 +- config/rpng_aruco/estimator_config.yaml | 2 +- config/rpng_ironsides/estimator_config.yaml | 2 +- config/rpng_sim/estimator_config.yaml | 4 +- config/tum_vi/estimator_config.yaml | 2 +- config/uzhfpv_indoor/estimator_config.yaml | 2 +- config/uzhfpv_indoor_45/estimator_config.yaml | 2 +- ov_core/src/utils/sensor_data.h | 46 +- ov_eval/src/error_simulation.cpp | 57 +- ov_msckf/launch/simulation.launch | 12 +- ov_msckf/src/core/VioManager.cpp | 41 +- ov_msckf/src/core/VioManagerOptions.h | 28 +- ov_msckf/src/ros/RosVisualizerHelper.h | 38 +- ov_msckf/src/sim/Simulator.cpp | 38 +- ov_msckf/src/state/Propagator.cpp | 163 ++-- ov_msckf/src/state/Propagator.h | 801 +++++++++--------- ov_msckf/src/state/State.h | 6 +- ov_msckf/src/state/StateOptions.h | 13 +- ov_msckf/src/update/UpdaterZeroVelocity.cpp | 12 +- ov_msckf/src/update/UpdaterZeroVelocity.h | 6 - 23 files changed, 630 insertions(+), 653 deletions(-) diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index 292207915..e6196b9d4 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: true # if rk4 integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) diff --git a/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml index c69d139cb..cf2779a7a 100644 --- a/config/kaist/estimator_config.yaml +++ b/config/kaist/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/config/kaist/pgeneva_ros_kaist.launch.old b/config/kaist/pgeneva_ros_kaist.launch.old index 4cb02c993..09ed809ee 100644 --- a/config/kaist/pgeneva_ros_kaist.launch.old +++ b/config/kaist/pgeneva_ros_kaist.launch.old @@ -25,7 +25,7 @@ - + diff --git a/config/kaist_vio/pgeneva_ros_kaistvio.launch.old b/config/kaist_vio/pgeneva_ros_kaistvio.launch.old index 808a8ff8c..81ddd1d46 100644 --- a/config/kaist_vio/pgeneva_ros_kaistvio.launch.old +++ b/config/kaist_vio/pgeneva_ros_kaistvio.launch.old @@ -31,7 +31,7 @@ - + diff --git a/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml index 8977d2c5f..650bdbb5d 100644 --- a/config/rpng_aruco/estimator_config.yaml +++ b/config/rpng_aruco/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml index 5213701a1..418fb71f7 100644 --- a/config/rpng_ironsides/estimator_config.yaml +++ b/config/rpng_ironsides/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index c01e03dff..ea1e53e8b 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -3,8 +3,8 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_imuavg: false # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml index a62d4b015..74c2a953e 100644 --- a/config/tum_vi/estimator_config.yaml +++ b/config/tum_vi/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml index fdf9ff6c5..be9bde714 100644 --- a/config/uzhfpv_indoor/estimator_config.yaml +++ b/config/uzhfpv_indoor/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml index fdf9ff6c5..be9bde714 100644 --- a/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/config/uzhfpv_indoor_45/estimator_config.yaml @@ -4,7 +4,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: false # if rk4 integration should be used (overrides imu averaging) +use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index 8b5406bed..0975243ae 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -88,37 +88,33 @@ struct ImuConfig { /// imu model, 0: Kalibr model and 1: RPNG model int imu_model = 0; - /// imu intrinsics - Eigen::Matrix imu_x_dw; /// the columnwise elements for Dw - Eigen::Matrix imu_x_da; /// the columnwise elements for Da - Eigen::Matrix imu_x_tg; /// the ccolumnwise elements for Tg - Eigen::Matrix imu_quat_AcctoI; /// the JPL quat for R_AcctoI - Eigen::Matrix imu_quat_GyrotoI; /// the JPL quat for R_GyrotoI + /// the columnwise elements for Dw + Eigen::Matrix vec_dw; + + /// the columnwise elements for Da + Eigen::Matrix vec_da; + + /// the ccolumnwise elements for Tg + Eigen::Matrix vec_tg; + + /// the JPL quat for R_AcctoI + Eigen::Matrix q_ACCtoIMU; + + /// the JPL quat for R_GyrotoI + Eigen::Matrix q_GYROtoIMU; /// Gyroscope white noise (rad/s/sqrt(hz)) double sigma_w = 1.6968e-04; - /// Gyroscope white noise covariance - double sigma_w_2 = pow(1.6968e-04, 2); - /// Gyroscope random walk (rad/s^2/sqrt(hz)) double sigma_wb = 1.9393e-05; - /// Gyroscope random walk covariance - double sigma_wb_2 = pow(1.9393e-05, 2); - /// Accelerometer white noise (m/s^2/sqrt(hz)) double sigma_a = 2.0000e-3; - /// Accelerometer white noise covariance - double sigma_a_2 = pow(2.0000e-3, 2); - /// Accelerometer random walk (m/s^3/sqrt(hz)) double sigma_ab = 3.0000e-03; - /// Accelerometer random walk covariance - double sigma_ab_2 = pow(3.0000e-03, 2); - /// Nice print function of what parameters we have loaded void print() { PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); @@ -132,10 +128,10 @@ struct ImuConfig { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); if (imu_model == 0) { // Kalibr model with lower triangular of the matrix - Dw << imu_x_dw(0), 0, 0, imu_x_dw(1), imu_x_dw(3), 0, imu_x_dw(2), imu_x_dw(4), imu_x_dw(5); + Dw << vec_dw(0), 0, 0, vec_dw(1), vec_dw(3), 0, vec_dw(2), vec_dw(4), vec_dw(5); } else { // rpng model with upper triangular of the matrix - Dw << imu_x_dw(0), imu_x_dw(1), imu_x_dw(3), 0, imu_x_dw(2), imu_x_dw(4), 0, 0, imu_x_dw(5); + Dw << vec_dw(0), vec_dw(1), vec_dw(3), 0, vec_dw(2), vec_dw(4), 0, 0, vec_dw(5); } return Dw; } @@ -145,10 +141,10 @@ struct ImuConfig { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); if (imu_model == 0) { // kalibr model with lower triangular of the matrix - Da << imu_x_da(0), 0, 0, imu_x_da(1), imu_x_da(3), 0, imu_x_da(2), imu_x_da(4), imu_x_da(5); + Da << vec_da(0), 0, 0, vec_da(1), vec_da(3), 0, vec_da(2), vec_da(4), vec_da(5); } else { // rpng model with upper triangular of the matrix - Da << imu_x_da(0), imu_x_da(1), imu_x_da(3), 0, imu_x_da(2), imu_x_da(4), 0, 0, imu_x_da(5); + Da << vec_da(0), vec_da(1), vec_da(3), 0, vec_da(2), vec_da(4), 0, 0, vec_da(5); } return Da; } @@ -162,17 +158,17 @@ struct ImuConfig { /// get the IMU Tg Eigen::Matrix3d Tg() { Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << imu_x_tg(0), imu_x_tg(3), imu_x_tg(6), imu_x_tg(1), imu_x_tg(4), imu_x_tg(7), imu_x_tg(2), imu_x_tg(5), imu_x_tg(8); + Tg << vec_tg(0), vec_tg(3), vec_tg(6), vec_tg(1), vec_tg(4), vec_tg(7), vec_tg(2), vec_tg(5), vec_tg(8); return Tg; } /// get the R_AcctoI - Eigen::Matrix3d R_AcctoI() { return ov_core::quat_2_Rot(imu_quat_AcctoI); } + Eigen::Matrix3d R_AcctoI() { return ov_core::quat_2_Rot(q_ACCtoIMU); } /// get the R_ItoAcc Eigen::Matrix3d R_ItoAcc() { return R_AcctoI().transpose(); } /// get the R_GyrotoI - Eigen::Matrix3d R_GyrotoI() { return ov_core::quat_2_Rot(imu_quat_GyrotoI); } + Eigen::Matrix3d R_GyrotoI() { return ov_core::quat_2_Rot(q_GYROtoIMU); } /// get the R_ItoGyro Eigen::Matrix3d R_ItoGyro() { return R_GyrotoI().transpose(); } diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp index 62eb8d0a4..663a482aa 100644 --- a/ov_eval/src/error_simulation.cpp +++ b/ov_eval/src/error_simulation.cpp @@ -34,44 +34,43 @@ int main(int argc, char **argv) { - // Verbosity setting - ov_core::Printer::setPrintLevel("INFO"); + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); - // Ensure we have a path - if (argc < 4) { - PRINT_ERROR(RED "ERROR: ./error_simulation \n" RESET); - PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation \n" RESET); - std::exit(EXIT_FAILURE); - } + // Ensure we have a path + if (argc < 4) { + PRINT_ERROR(RED "ERROR: ./error_simulation \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation \n" RESET); + std::exit(EXIT_FAILURE); + } - // Create our trajectory object - ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]); + // Create our trajectory object + ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]); - // Plot the state errors - PRINT_INFO("Plotting state variable errors...\n"); - traj.plot_state(true); + // Plot the state errors + PRINT_INFO("Plotting state variable errors...\n"); + traj.plot_state(true); - // Plot time offset - PRINT_INFO("Plotting time offset error...\n"); - traj.plot_timeoff(true, 10); + // Plot time offset + PRINT_INFO("Plotting time offset error...\n"); + traj.plot_timeoff(true, 10); - // Plot camera intrinsics - PRINT_INFO("Plotting camera intrinsics...\n"); - traj.plot_cam_instrinsics(true, 60); + // Plot camera intrinsics + PRINT_INFO("Plotting camera intrinsics...\n"); + traj.plot_cam_instrinsics(true, 60); - // Plot camera extrinsics - PRINT_INFO("Plotting camera extrinsics...\n"); - traj.plot_cam_extrinsics(true, 60); + // Plot camera extrinsics + PRINT_INFO("Plotting camera extrinsics...\n"); + traj.plot_cam_extrinsics(true, 60); - - // Plot IMU intrinsics - PRINT_INFO("Plotting IMU intrinsics...\n"); - traj.plot_imu_intrinsics(true); + // Plot IMU intrinsics + PRINT_INFO("Plotting IMU intrinsics...\n"); + traj.plot_imu_intrinsics(true); #ifdef HAVE_PYTHONLIBS - matplotlibcpp::show(true); + matplotlibcpp::show(true); #endif - // Done! - return EXIT_SUCCESS; + // Done! + return EXIT_SUCCESS; } diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index e2e12ee55..b4cb53663 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -5,12 +5,12 @@ - + - + @@ -24,10 +24,10 @@ - - - - + + + + diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 48eaba0fc..9d907051b 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -48,11 +48,11 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { state = std::make_shared(params.state_options); // Set the IMU intrinsics - state->_calib_imu_dw->set_value(params.imu_config.imu_x_dw); - state->_calib_imu_da->set_value(params.imu_config.imu_x_da); - state->_calib_imu_tg->set_value(params.imu_config.imu_x_tg); - state->_calib_imu_GYROtoIMU->set_value(params.imu_config.imu_quat_GyrotoI); - state->_calib_imu_ACCtoIMU->set_value(params.imu_config.imu_quat_AcctoI); + state->_calib_imu_dw->set_value(params.imu_config.vec_dw); + state->_calib_imu_da->set_value(params.imu_config.vec_da); + state->_calib_imu_tg->set_value(params.imu_config.vec_tg); + state->_calib_imu_GYROtoIMU->set_value(params.imu_config.q_GYROtoIMU); + state->_calib_imu_ACCtoIMU->set_value(params.imu_config.q_ACCtoIMU); // Timeoffset from camera to IMU Eigen::VectorXd temp_camimu_dt; @@ -724,24 +724,33 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) } // Debug for imu intrinsics - if (state->_options.do_calib_imu_g_sensitivity) { - PRINT_INFO("Tg = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_tg->value()(0), state->_calib_imu_tg->value()(1), - state->_calib_imu_tg->value()(2), state->_calib_imu_tg->value()(3), state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), - state->_calib_imu_tg->value()(6), state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); - } if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { PRINT_INFO("Dw = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), - state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); + state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), + state->_calib_imu_dw->value()(5)); PRINT_INFO("Da = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), - state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); - PRINT_INFO("q_GYROtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_GYROtoIMU->value()(0), state->_calib_imu_GYROtoIMU->value()(1), - state->_calib_imu_GYROtoIMU->value()(2), state->_calib_imu_GYROtoIMU->value()(3)); + state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), + state->_calib_imu_da->value()(5)); } if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { PRINT_INFO("Dw = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), - state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); + state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), + state->_calib_imu_dw->value()(5)); PRINT_INFO("Da = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), - state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); + state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), + state->_calib_imu_da->value()(5)); + } + if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { + PRINT_INFO("Tg = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_tg->value()(0), + state->_calib_imu_tg->value()(1), state->_calib_imu_tg->value()(2), state->_calib_imu_tg->value()(3), + state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), state->_calib_imu_tg->value()(6), + state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); + } + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { + PRINT_INFO("q_GYROtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_GYROtoIMU->value()(0), state->_calib_imu_GYROtoIMU->value()(1), + state->_calib_imu_GYROtoIMU->value()(2), state->_calib_imu_GYROtoIMU->value()(3)); + } + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { PRINT_INFO("q_ACCtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_ACCtoIMU->value()(0), state->_calib_imu_ACCtoIMU->value()(1), state->_calib_imu_ACCtoIMU->value()(2), state->_calib_imu_ACCtoIMU->value()(3)); } diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 0c7b0cd98..5ab704257 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -157,10 +157,6 @@ struct VioManagerOptions { parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_config.sigma_wb); parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_config.sigma_a); parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_config.sigma_ab); - imu_config.sigma_w_2 = std::pow(imu_config.sigma_w, 2); - imu_config.sigma_wb_2 = std::pow(imu_config.sigma_wb, 2); - imu_config.sigma_a_2 = std::pow(imu_config.sigma_a, 2); - imu_config.sigma_ab_2 = std::pow(imu_config.sigma_ab, 2); } imu_config.print(); if (parser != nullptr) { @@ -317,15 +313,15 @@ struct VioManagerOptions { // kalibr model: lower triangular of the matrix and R_GYROtoI // rpng model: upper triangular of the matrix and R_ACCtoI if (imu_config.imu_model == 0) { - imu_config.imu_x_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); - imu_config.imu_x_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); + imu_config.vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); + imu_config.vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); } else { - imu_config.imu_x_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2); - imu_config.imu_x_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); + imu_config.vec_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2); + imu_config.vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); } - imu_config.imu_x_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2); - imu_config.imu_quat_GyrotoI = ov_core::rot_2_quat(R_GyrotoI); - imu_config.imu_quat_AcctoI = ov_core::rot_2_quat(R_AcctoI); + imu_config.vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2); + imu_config.q_GYROtoIMU = ov_core::rot_2_quat(R_GyrotoI); + imu_config.q_ACCtoIMU = ov_core::rot_2_quat(R_AcctoI); } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); @@ -359,11 +355,11 @@ struct VioManagerOptions { } PRINT_DEBUG("IMU PARAMETERS:\n"); std::stringstream ss; - ss << "Dw (columnwise):" << std::endl << imu_config.imu_x_dw.transpose() << std::endl; - ss << "Da (columnwise):" << std::endl << imu_config.imu_x_da.transpose() << std::endl; - ss << "Tg (columnwise):" << std::endl << imu_config.imu_x_tg.transpose() << std::endl; - ss << "R_GYROtoI:" << std::endl << imu_config.imu_quat_GyrotoI << std::endl; - ss << "R_ACCtoI:" << std::endl << imu_config.imu_quat_AcctoI << std::endl; + ss << "Dw (columnwise):" << imu_config.vec_dw.transpose() << std::endl; + ss << "Da (columnwise):" << imu_config.vec_da.transpose() << std::endl; + ss << "Tg (columnwise):" << imu_config.vec_tg.transpose() << std::endl; + ss << "q_GYROtoI: " << imu_config.q_GYROtoIMU.transpose() << std::endl; + ss << "q_ACCtoI: " << imu_config.q_ACCtoIMU.transpose() << std::endl; PRINT_DEBUG(ss.str().c_str()); } diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 5fa71090f..49fda921b 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -286,25 +286,25 @@ class RosVisualizerHelper { of_state_gt << sim->get_true_parameters().imu_config.imu_model << " "; of_state_gt.precision(8); - of_state_gt << sim->get_true_parameters().imu_config.imu_x_dw(0) << " " << sim->get_true_parameters().imu_config.imu_x_dw(1) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(2) << " " << sim->get_true_parameters().imu_config.imu_x_dw(3) << " " - << sim->get_true_parameters().imu_config.imu_x_dw(4) << " " << sim->get_true_parameters().imu_config.imu_x_dw(5) << " "; - of_state_gt << sim->get_true_parameters().imu_config.imu_x_da(0) << " " << sim->get_true_parameters().imu_config.imu_x_da(1) << " " - << sim->get_true_parameters().imu_config.imu_x_da(2) << " " << sim->get_true_parameters().imu_config.imu_x_da(3) << " " - << sim->get_true_parameters().imu_config.imu_x_da(4) << " " << sim->get_true_parameters().imu_config.imu_x_da(5) << " "; - of_state_gt << sim->get_true_parameters().imu_config.imu_x_tg(0) << " " << sim->get_true_parameters().imu_config.imu_x_tg(1) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(2) << " " << sim->get_true_parameters().imu_config.imu_x_tg(3) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(4) << " " << sim->get_true_parameters().imu_config.imu_x_tg(5) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(6) << " " << sim->get_true_parameters().imu_config.imu_x_tg(7) << " " - << sim->get_true_parameters().imu_config.imu_x_tg(8) << " "; - of_state_gt << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(0) << " " - << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(1) << " " - << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(2) << " " - << sim->get_true_parameters().imu_config.imu_quat_GyrotoI(3) << " "; - of_state_gt << sim->get_true_parameters().imu_config.imu_quat_AcctoI(0) << " " - << sim->get_true_parameters().imu_config.imu_quat_AcctoI(1) << " " - << sim->get_true_parameters().imu_config.imu_quat_AcctoI(2) << " " - << sim->get_true_parameters().imu_config.imu_quat_AcctoI(3) << " "; + of_state_gt << sim->get_true_parameters().imu_config.vec_dw(0) << " " << sim->get_true_parameters().imu_config.vec_dw(1) << " " + << sim->get_true_parameters().imu_config.vec_dw(2) << " " << sim->get_true_parameters().imu_config.vec_dw(3) << " " + << sim->get_true_parameters().imu_config.vec_dw(4) << " " << sim->get_true_parameters().imu_config.vec_dw(5) << " "; + of_state_gt << sim->get_true_parameters().imu_config.vec_da(0) << " " << sim->get_true_parameters().imu_config.vec_da(1) << " " + << sim->get_true_parameters().imu_config.vec_da(2) << " " << sim->get_true_parameters().imu_config.vec_da(3) << " " + << sim->get_true_parameters().imu_config.vec_da(4) << " " << sim->get_true_parameters().imu_config.vec_da(5) << " "; + of_state_gt << sim->get_true_parameters().imu_config.vec_tg(0) << " " << sim->get_true_parameters().imu_config.vec_tg(1) << " " + << sim->get_true_parameters().imu_config.vec_tg(2) << " " << sim->get_true_parameters().imu_config.vec_tg(3) << " " + << sim->get_true_parameters().imu_config.vec_tg(4) << " " << sim->get_true_parameters().imu_config.vec_tg(5) << " " + << sim->get_true_parameters().imu_config.vec_tg(6) << " " << sim->get_true_parameters().imu_config.vec_tg(7) << " " + << sim->get_true_parameters().imu_config.vec_tg(8) << " "; + of_state_gt << sim->get_true_parameters().imu_config.q_GYROtoIMU(0) << " " + << sim->get_true_parameters().imu_config.q_GYROtoIMU(1) << " " + << sim->get_true_parameters().imu_config.q_GYROtoIMU(2) << " " + << sim->get_true_parameters().imu_config.q_GYROtoIMU(3) << " "; + of_state_gt << sim->get_true_parameters().imu_config.q_ACCtoIMU(0) << " " + << sim->get_true_parameters().imu_config.q_ACCtoIMU(1) << " " + << sim->get_true_parameters().imu_config.q_ACCtoIMU(2) << " " + << sim->get_true_parameters().imu_config.q_ACCtoIMU(3) << " "; // New line of_state_gt << endl; } diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 3c4f44918..58a1184d8 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -227,28 +227,28 @@ void Simulator::perturb_parameters(std::mt19937 gen_state, VioManagerOptions &pa } } - // perturb the imu intrinsics - for (int j = 0; j < 6; j++) { - params_.imu_config.imu_x_dw(j) += 0.004 * w(gen_state); - params_.imu_config.imu_x_da(j) += 0.004 * w(gen_state); - } - - // if we need to perturb gravity sensitivity - for (int j = 0; j < 5; j++) { - if (params_.state_options.do_calib_imu_g_sensitivity) { - params_.imu_config.imu_x_tg(j) += 0.004 * w(gen_state); + // If we need to perturb the imu intrinsics + if (params_.state_options.do_calib_imu_intrinsics) { + for (int j = 0; j < 6; j++) { + params_.imu_config.vec_dw(j) += 0.004 * w(gen_state); + params_.imu_config.vec_da(j) += 0.004 * w(gen_state); + } + if (params_.state_options.imu_model == 0) { + Eigen::Vector3d w_vec; + w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); + params_.imu_config.q_GYROtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.q_GYROtoIMU)); + } else { + Eigen::Vector3d w_vec; + w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); + params_.imu_config.q_ACCtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.q_ACCtoIMU)); } } - // perturb imu intrinsics (depends on model type) - if (params_.state_options.imu_model == 0) { - Eigen::Vector3d w_vec; - w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); - params_.imu_config.imu_quat_GyrotoI = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.imu_quat_GyrotoI)); - } else { - Eigen::Vector3d w_vec; - w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); - params_.imu_config.imu_quat_AcctoI = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.imu_quat_AcctoI)); + // If we need to perturb gravity sensitivity + if (params_.state_options.do_calib_imu_g_sensitivity) { + for (int j = 0; j < 9; j++) { + params_.imu_config.vec_tg(j) += 0.004 * w(gen_state); + } } } diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 0e0e82f79..9d7bf067d 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -96,7 +96,8 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest Eigen::Vector3d last_w = Eigen::Vector3d::Zero(); if (!prop_data.empty()) { last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); - Eigen::Vector3d last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; + // TODO: is this acceleration used correct? + // Eigen::Vector3d last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; last_w = state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); } @@ -157,9 +158,9 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // assert(data_plus.timestamp>data_minus.timestamp); // Corrected imu acc measurements with our current biases - Eigen::Matrix a_hat1 = data_minus.am - state->_imu->bias_a(); - Eigen::Matrix a_hat2 = data_plus.am - state->_imu->bias_a(); - Eigen::Matrix a_hat = a_hat1; + Eigen::Vector3d a_hat1 = data_minus.am - state->_imu->bias_a(); + Eigen::Vector3d a_hat2 = data_plus.am - state->_imu->bias_a(); + Eigen::Vector3d a_hat = a_hat1; if (state->_options.imu_avg) { a_hat = .5 * (a_hat1 + a_hat2); } @@ -171,20 +172,17 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; // gravity sensitivity correction - Eigen::Matrix gravity_correction1 = state->Tg() * a_hat1; - Eigen::Matrix gravity_correction2 = state->Tg() * a_hat2; + Eigen::Vector3d gravity_correction1 = state->Tg() * a_hat1; + Eigen::Vector3d gravity_correction2 = state->Tg() * a_hat2; // Corrected imu gyro measurements with our current biases - Eigen::Matrix w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; - Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; - Eigen::Matrix w_hat = w_hat1; + Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; + Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; + Eigen::Vector3d w_hat = w_hat1; if (state->_options.imu_avg) { w_hat = .5 * (w_hat1 + w_hat2); } - // Imu readings in raw frame - Eigen::Vector3d w_uncorrected = w_hat; - // Correct imu readings with IMU intrinsics w_hat = state->R_GyrotoI() * state->Dw() * w_hat; w_hat1 = state->R_GyrotoI() * state->Dw() * w_hat1; @@ -199,12 +197,13 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if (state->_options.use_analytic_integration) + if (state->_options.use_analytic_integration) { predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); - else if (state->_options.use_rk4_integration) + } else if (state->_options.use_rk4_integration) { predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); - else + } else { predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + } // Now replace imu estimate and fej with propagated values Eigen::Matrix imu_x = state->_imu->value(); @@ -217,14 +216,16 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times } // Now record what the predicted state should be + // TODO: apply IMU intrinsics here to correct the angular velocity state_plus = Eigen::Matrix::Zero(); state_plus.block(0, 0, 4, 1) = state->_imu->quat(); state_plus.block(4, 0, 3, 1) = state->_imu->pos(); state_plus.block(7, 0, 3, 1) = state->_imu->vel(); - if (prop_data.size() > 1) + if (prop_data.size() > 1) { state_plus.block(10, 0, 3, 1) = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); - else if (!prop_data.empty()) + } else if (!prop_data.empty()) { state_plus.block(10, 0, 3, 1) = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); + } // Finally replace the imu with the original state we had state->_imu->set_value(orig_val); @@ -353,64 +354,62 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Set them to zero F.setZero(); + Qd.setZero(); // Time elapsed over interval double dt = data_plus.timestamp - data_minus.timestamp; // assert(data_plus.timestamp>data_minus.timestamp); // Corrected imu acc measurements with our current biases - Eigen::Matrix a_hat1 = data_minus.am - state->_imu->bias_a(); - Eigen::Matrix a_hat2 = data_plus.am - state->_imu->bias_a(); - Eigen::Matrix a_hat = a_hat1; + Eigen::Vector3d a_hat1 = data_minus.am - state->_imu->bias_a(); + Eigen::Vector3d a_hat2 = data_plus.am - state->_imu->bias_a(); + Eigen::Vector3d a_hat = a_hat1; if (state->_options.imu_avg) { a_hat = .5 * (a_hat1 + a_hat2); } - // Imu acc readings in raw frame + + // Convert "raw" imu to its corrected frame using the IMU intrinsics Eigen::Vector3d a_uncorrected = a_hat; - // Correct imu readings with IMU intrinsics a_hat = state->R_AcctoI() * state->Da() * a_hat; a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; - // gravity sensitivity correction - Eigen::Matrix gravity_correction1 = state->Tg() * a_hat1; - Eigen::Matrix gravity_correction2 = state->Tg() * a_hat2; - - // Corrected imu gyro measurements with our current biases - Eigen::Matrix w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; - Eigen::Matrix w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; - Eigen::Matrix w_hat = w_hat1; + // Corrected imu gyro measurements with our current biases and gravity sensativity + Eigen::Vector3d gravity_correction1 = state->Tg() * a_hat1; + Eigen::Vector3d gravity_correction2 = state->Tg() * a_hat2; + Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; + Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; + Eigen::Vector3d w_hat = w_hat1; if (state->_options.imu_avg) { w_hat = .5 * (w_hat1 + w_hat2); } - // Imu readings in raw frame - Eigen::Vector3d w_uncorrected = w_hat; - // Correct imu readings with IMU intrinsics + // Convert "raw" imu to its corrected frame using the IMU intrinsics + Eigen::Vector3d w_uncorrected = w_hat; w_hat = state->R_GyrotoI() * state->Dw() * w_hat; w_hat1 = state->R_GyrotoI() * state->Dw() * w_hat1; w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; - // Pre-compute some analytical values + // Pre-compute some analytical values for the mean and covariance integration Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); - if (state->_options.use_analytic_integration) { + if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); } // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if (state->_options.use_analytic_integration) + if (state->_options.use_analytic_integration) { predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); - else if (state->_options.use_rk4_integration) + } else if (state->_options.use_rk4_integration) { predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); - else + } else { predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + } // Allocate state transition and noise Jacobian F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, 12); - if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { compute_F_and_G_analytic(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G); } else { @@ -421,10 +420,10 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report Eigen::Matrix Qc = Eigen::Matrix::Zero(); - Qc.block(0, 0, 3, 3) = _imu_config.sigma_w_2 / dt * Eigen::Matrix::Identity(); - Qc.block(3, 3, 3, 3) = _imu_config.sigma_a_2 / dt * Eigen::Matrix::Identity(); - Qc.block(6, 6, 3, 3) = _imu_config.sigma_wb_2 / dt * Eigen::Matrix::Identity(); - Qc.block(9, 9, 3, 3) = _imu_config.sigma_ab_2 / dt * Eigen::Matrix::Identity(); + Qc.block(0, 0, 3, 3) = std::pow(_imu_config.sigma_w, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(3, 3, 3, 3) = std::pow(_imu_config.sigma_a, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(6, 6, 3, 3) = std::pow(_imu_config.sigma_wb, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(9, 9, 3, 3) = std::pow(_imu_config.sigma_ab, 2) / dt * Eigen::Matrix3d::Identity(); // Compute the noise injected into the state over the interval Qd = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); @@ -446,9 +445,6 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d const Eigen::Vector3d &new_p, const Eigen::Matrix &Xi_sum, Eigen::MatrixXd &F, Eigen::MatrixXd &G) { - //============================================================ - //============================================================ - // Get the locations of each entry of the imu state int local_size = 0; int th_id = local_size; @@ -477,40 +473,35 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Tg_id = local_size; local_size += state->_calib_imu_tg->size(); } + // 0: kalibr, 1: rpng if (state->_options.imu_model == 0) { - // Kalibr model th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); } else { - // RPNG model th_atoI_id = local_size; local_size += state->_calib_imu_ACCtoIMU->size(); } } - //============================================================ - //============================================================ - // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix R_k = state->_imu->Rot(); - Eigen::Matrix v_k = state->_imu->vel(); - Eigen::Matrix p_k = state->_imu->pos(); - + Eigen::Matrix3d R_k = state->_imu->Rot(); + Eigen::Vector3d v_k = state->_imu->vel(); + Eigen::Vector3d p_k = state->_imu->pos(); if (state->_options.do_fej) { R_k = state->_imu->Rot_fej(); v_k = state->_imu->vel_fej(); p_k = state->_imu->pos_fej(); } - Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); + Eigen::Matrix3d dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); - Eigen::Matrix Da = state->Da(); - Eigen::Matrix Dw = state->Dw(); - Eigen::Matrix Tg = state->Tg(); - Eigen::Matrix R_atoI = state->R_AcctoI(); - Eigen::Matrix R_wtoI = state->R_GyrotoI(); - Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; - Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected; // contains the gravity correction already + Eigen::Matrix3d Da = state->Da(); + Eigen::Matrix3d Dw = state->Dw(); + Eigen::Matrix3d Tg = state->Tg(); + Eigen::Matrix3d R_atoI = state->R_AcctoI(); + Eigen::Matrix3d R_wtoI = state->R_GyrotoI(); + Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; + Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains gravity correction already Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3); Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6); @@ -527,7 +518,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d F.block<3, 3>(p_id, p_id).setIdentity(); // for v - F.block<3, 3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; + F.block<3, 3>(p_id, v_id) = Eigen::Matrix3d::Identity() * dt; F.block<3, 3>(v_id, v_id).setIdentity(); // for bg @@ -591,8 +582,8 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d G.block<3, 3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; G.block<3, 3>(p_id, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; G.block<3, 3>(v_id, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; - G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix::Identity(); - G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix::Identity(); + G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix3d::Identity(); + G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix3d::Identity(); } void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, @@ -600,9 +591,6 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, Eigen::MatrixXd &F, Eigen::MatrixXd &G) { - //============================================================ - //============================================================ - // Get the locations of each entry of the imu state int local_size = 0; int th_id = local_size; @@ -647,27 +635,27 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix R_k = state->_imu->Rot(); - Eigen::Matrix v_k = state->_imu->vel(); - Eigen::Matrix p_k = state->_imu->pos(); + Eigen::Matrix3d R_k = state->_imu->Rot(); + Eigen::Vector3d v_k = state->_imu->vel(); + Eigen::Vector3d p_k = state->_imu->pos(); if (state->_options.do_fej) { R_k = state->_imu->Rot_fej(); v_k = state->_imu->vel_fej(); p_k = state->_imu->pos_fej(); } - Eigen::Matrix dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); + Eigen::Matrix3d dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix Da = state->Da(); - Eigen::Matrix Dw = state->Dw(); - Eigen::Matrix Tg = state->Tg(); - Eigen::Matrix R_atoI = state->R_AcctoI(); - Eigen::Matrix R_wtoI = state->R_GyrotoI(); - Eigen::Matrix a_k = R_atoI * Da * a_uncorrected; - Eigen::Matrix w_k = R_wtoI * Dw * w_uncorrected; // contains the gravity correction already - Eigen::Matrix Jr = Jr_so3(w_k * dt); + Eigen::Matrix3d Da = state->Da(); + Eigen::Matrix3d Dw = state->Dw(); + Eigen::Matrix3d Tg = state->Tg(); + Eigen::Matrix3d R_atoI = state->R_AcctoI(); + Eigen::Matrix3d R_wtoI = state->R_GyrotoI(); + Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; + Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains the gravity correction already + Eigen::Matrix3d Jr = Jr_so3(w_k * dt); // for theta F.block<3, 3>(th_id, th_id) = dR_ktok1; @@ -678,7 +666,7 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // for position F.block<3, 3>(p_id, th_id).noalias() = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); F.block<3, 3>(p_id, p_id).setIdentity(); - F.block<3, 3>(p_id, v_id) = Eigen::Matrix::Identity() * dt; + F.block<3, 3>(p_id, v_id) = Eigen::Matrix3d::Identity() * dt; F.block<3, 3>(p_id, ba_id) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; // for velocity @@ -730,8 +718,8 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d G.block<3, 3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; G.block<3, 3>(v_id, 3) = -R_k.transpose() * dt * R_atoI * Da; G.block<3, 3>(p_id, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; - G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix::Identity(); - G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix::Identity(); + G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix3d::Identity(); + G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix3d::Identity(); } void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, @@ -749,7 +737,7 @@ void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, // Pre-compute things double w_norm = w_hat.norm(); Eigen::Matrix I_4x4 = Eigen::Matrix::Identity(); - Eigen::Matrix R_Gtoi = state->_imu->Rot(); + Eigen::Matrix3d R_Gtoi = state->_imu->Rot(); // Orientation: Equation (101) and (103) and of Trawny indirect TR Eigen::Matrix bigO; @@ -773,7 +761,7 @@ void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, Eigen::Matrix &Xi_sum) { // Pre-compute things - Eigen::Matrix R_Gtok = state->_imu->Rot(); + Eigen::Matrix3d R_Gtok = state->_imu->Rot(); // get the pre-computed value Eigen::Matrix3d R_k1_to_k = Xi_sum.block<3, 3>(0, 0); @@ -1022,15 +1010,12 @@ Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eig Eigen::MatrixXd Propagator::compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI) { Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); - Eigen::Vector3d e_1 = I_3x3.block<3, 1>(0, 0); - Eigen::Vector3d e_2 = I_3x3.block<3, 1>(0, 1); double a_1 = a_inI(0); double a_2 = a_inI(1); double a_3 = a_inI(2); // intrinsic parameters Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3, 9); - if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { H_Tg << a_1 * I_3x3, a_2 * I_3x3, a_3 * I_3x3; } diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 225ae3166..2c60c084f 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -36,412 +36,401 @@ namespace ov_msckf { * We then compute the state transition matrix at each step and update the state and covariance. * For derivations look at @ref propagation page which has detailed equations. */ - class Propagator { - - public: - - /** - * @brief Default constructor - * @param noises imu noise characteristics (continuous time) - * @param gravity_mag Global gravity magnitude of the system (normally 9.81) - */ - Propagator( ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { - _imu_config.sigma_w_2 = std::pow(_imu_config.sigma_w, 2); - _imu_config.sigma_a_2 = std::pow(_imu_config.sigma_a, 2); - _imu_config.sigma_wb_2 = std::pow(_imu_config.sigma_wb, 2); - _imu_config.sigma_ab_2 = std::pow(_imu_config.sigma_ab, 2); - last_prop_time_offset = 0.0; - _gravity << 0.0, 0.0, gravity_mag; - } - - /** - * @brief Stores incoming inertial readings - * @param message Contains our timestamp and inertial information - */ - void feed_imu(const ov_core::ImuData &message) { - - // Append it to our vector - imu_data.emplace_back(message); - - // Loop through and delete imu messages that are older then 10 seconds - // TODO: we should probably have more elegant logic then this - // TODO: but this prevents unbounded memory growth and slow prop with high freq imu - auto it0 = imu_data.begin(); - while (it0 != imu_data.end()) { - if (message.timestamp - (*it0).timestamp > 10) { - it0 = imu_data.erase(it0); - } else { - it0++; - } - } - } - - /** - * @brief Propagate state up to given timestamp and then clone - * - * This will first collect all imu readings that occured between the - * *current* state time and the new time we want the state to be at. - * If we don't have any imu readings we will try to extrapolate into the future. - * After propagating the mean and covariance using our dynamics, - * We clone the current imu pose as a new clone in our state. - * - * @param state Pointer to state - * @param timestamp Time to propagate to and clone at - */ - void propagate_and_clone(std::shared_ptr state, double timestamp); - - /** - * @brief Gets what the state and its covariance will be at a given timestamp - * - * This can be used to find what the state will be in the "future" without propagating it. - * This will propagate a clone of the current IMU state and its covariance matrix. - * This is typically used to provide high frequency pose estimates between updates. - * - * @param state Pointer to state - * @param timestamp Time to propagate to - * @param state_plus The propagated state (q_GtoI, p_IinG, v_IinG, w_IinI) - */ - void fast_state_propagate(std::shared_ptr state, double timestamp, Eigen::Matrix &state_plus); - - /** - * @brief Helper function that given current imu data, will select imu readings between the two times. - * - * This will create measurements that we will integrate with, and an extra measurement at the end. - * We use the @ref interpolate_data() function to "cut" the imu readings at the begining and end of the integration. - * The timestamps passed should already take into account the time offset values. - * - * @param imu_data IMU data we will select measurements from - * @param time0 Start timestamp - * @param time1 End timestamp - * @param warn If we should warn if we don't have enough IMU to propagate with (e.g. fast prop will get warnings otherwise) - * @return Vector of measurements (if we could compute them) - */ - static std::vector select_imu_readings(const std::vector &imu_data, double time0, double time1, - bool warn = true); - - /** - * @brief Nice helper function that will linearly interpolate between two imu messages. - * - * This should be used instead of just "cutting" imu messages that bound the camera times - * Give better time offset if we use this function, could try other orders/splines if the imu is slow. - * - * @param imu_1 imu at begining of interpolation interval - * @param imu_2 imu at end of interpolation interval - * @param timestamp Timestamp being interpolated to - */ - static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) { - // time-distance lambda - double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp); - // PRINT_DEBUG("lambda - %d\n", lambda); - // interpolate between the two times - ov_core::ImuData data; - data.timestamp = timestamp; - data.am = (1 - lambda) * imu_1.am + lambda * imu_2.am; - data.wm = (1 - lambda) * imu_1.wm + lambda * imu_2.wm; - return data; - } - - protected: - /// Estimate for time offset at last propagation time - double last_prop_time_offset = 0.0; - bool have_last_prop_time_offset = false; - - /** - * @brief Propagates the state forward using the imu data and computes the noise covariance and state-transition - * matrix of this interval. - * - * This function can be replaced with analytical/numerical integration or when using a different state representation. - * This contains our state transition matrix along with how our noise evolves in time. - * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref error_prop page for details on how discrete model was derived. - * See the @ref imu_intrinsic page for details on how analytic model was derived. - * - * @param state Pointer to state - * @param data_minus imu readings at beginning of interval - * @param data_plus imu readings at end of interval - * @param F State-transition matrix over the interval - * @param Qd Discrete-time noise covariance over the interval - */ - void predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, - Eigen::MatrixXd &F, Eigen::MatrixXd &Qd); - - /** - * @brief Discrete imu mean propagation. - * - * See @ref propagation for details on these equations. - * \f{align*}{ - * \text{}^{I_{k+1}}_{G}\hat{\bar{q}} - * &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) - * \text{}^{I_{k}}_{G}\hat{\bar{q}} \\ - * ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t - * +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ - * ^G\hat{\mathbf{p}}_{I_{k+1}} - * &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t - * - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 - * + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 - * \f} - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat1 Angular velocity with bias removed - * @param a_hat1 Linear acceleration with bias removed - * @param w_hat2 Next angular velocity with bias removed - * @param a_hat2 Next linear acceleration with bias removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - */ - void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, - const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, - Eigen::Vector3d &new_p); - - /** - * @brief RK4 imu mean propagation. - * - * See this wikipedia page on [Runge-Kutta Methods](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods). - * We are doing a RK4 method, [this wolframe page](http://mathworld.wolfram.com/Runge-KuttaMethod.html) has the forth order equation - * defined below. We define function \f$ f(t,y) \f$ where y is a function of time t, see @ref imu_kinematic for the definition of the - * continous-time functions. - * - * \f{align*}{ - * {k_1} &= f({t_0}, y_0) \Delta t \\ - * {k_2} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_1} ) \Delta t \\ - * {k_3} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_2} ) \Delta t \\ - * {k_4} &= f( {t_0} + {\Delta t}, y_0 + {k_3} ) \Delta t \\ - * y_{0+\Delta t} &= y_0 + \left( {{1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4}} \right) - * \f} - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat1 Angular velocity with bias removed - * @param a_hat1 Linear acceleration with bias removed - * @param w_hat2 Next angular velocity with bias removed - * @param a_hat2 Next linear acceleration with bias removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - */ - void predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, - const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, - Eigen::Vector3d &new_p); - - - /** - * @brief Analytically compute the integration components based on ACI^2 - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) - * - * For computing Xi_1, Xi_2, Xi_3 and Xi_4, you can refer to @ref imu_intrinsics or the following equations: - * \f{align*}{ - * \boldsymbol{\Xi}_1 & = \mathbf{I}_3 \delta t + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} \lfloor \hat{\mathbf{k}} \rfloor - * + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 - * \boldsymbol{\Xi}_2 & = \frac{1}{2} \delta t^2 \mathbf{I}_3 + - * \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor - * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 - * \boldsymbol{\Xi}_3 &= - * \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor - * + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor - * + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} - * \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor - * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor ^2 - * + \left( - * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} - * \right) - * \lfloor \hat{\mathbf{k}} \rfloor ^2 \lfloor\hat{\mathbf{a}} \rfloor - * + \left( - * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} - * \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor - * - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} - * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 - * \boldsymbol{\Xi}_4 - * & = \frac{1}{6}\delta t^3 \lfloor\hat{\mathbf{a}} \rfloor - * + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} - * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor - * + \left( - * \frac{2(1- \cos(\hat{\omega} \delta t)) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t)}{\hat{\omega}^3} - * \right) - * \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor - * + \left( - * \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t}{\hat{\omega}^3} + - * \frac{\delta t^3}{6} - * \right) - * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 - * + - * \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} - * \lfloor \hat{\mathbf{k}} \rfloor^2\lfloor\hat{\mathbf{a}} \rfloor - * + - * \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} - * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor - * + - * \frac{4 \cos(\hat{\omega} \delta t) - 4 + (\hat{\omega} \delta t)^2 + \hat{\omega} \delta t \sin(\hat{\omega} \delta t) } - * {\hat{\omega}^3} - * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 - * \f} - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 in order - */ - void compute_Xi_sum(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Matrix &Xi_sum); - - - /** - * @brief Analytically predict IMU mean based on ACI^2 - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) - * - * \f{align*}{ - * {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}^{\top}_k {}^{I_k}_G\hat{\mathbf{R}} \\ - * {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\delta t_k + {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{p}}_k - \frac{1}{2}{}^G\mathbf{g}\delta t^2_k \\ - * {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{v}}_k - {}^G\mathbf{g}\delta t_k - * \f} - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 - */ - void predict_mean_analytic(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, - Eigen::Matrix &Xi_sum); - - - /** - * @brief Analytically compute state transition matrix F and noise Jacobian G based on ACI^2 - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) - * - * This function is for analytical integration or when using a different state representation. - * This contains our state transition matrix and noise Jacobians. - * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref imu_intrinsics page for details on how this was derived. - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 - * @param F State transition matrix - * @param G Noise Jacobian - */ - void compute_F_and_G_analytic(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, - const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, - const Eigen::Matrix &Xi_sum, - Eigen::MatrixXd &F, Eigen::MatrixXd &G); - - /** - * @brief compute state transition matrix F and noise Jacobian G - * - * This function is for analytical integration or when using a different state representation. - * This contains our state transition matrix and noise Jacobians. - * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref imu_intrinsics page for details on how this was derived. - * - * @param state Pointer to state - * @param dt Time we should integrate over - * @param w_hat Angular velocity with bias removed - * @param a_hat Linear acceleration with bias removed - * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed - * @param new_q The resulting new orientation after integration - * @param new_v The resulting new velocity after integration - * @param new_p The resulting new position after integration - * @param F State transition matrix - * @param G Noise Jacobian - */ - void compute_F_and_G_discrete(std::shared_ptr state, double dt, - const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, - const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, - Eigen::MatrixXd &F, Eigen::MatrixXd &G); - - /** - * @brief compute the Jacobians for Dw - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * For kalibr model - * \f{align*}{ - * \mathbf{H}_{Dw} & = - * \begin{bmatrix} - * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 - * \end{bmatrix} - * \f} - * For rpng model: - * \f{align*}{ - * \mathbf{H}_{Dw} & = - * \begin{bmatrix} - * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * @param state Pointer to state - * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed - */ - Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); - - /** - * @brief compute the Jacobians for Da - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * - * For kalibr model - * \f{align*}{ - * \mathbf{H}_{Da} & = - * \begin{bmatrix} - * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * For rpng: - * \f{align*}{ - * \mathbf{H}_{Da} & = - * \begin{bmatrix} - * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 - * \end{bmatrix} - * \f} - * @param state Pointer to state - * @param a_uncorrected Linear acceleration in gyro frame with bias removed - */ - Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); - - /** - * @brief compute the Jacobians for Tg - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * - * - * \f{align*}{ - * \mathbf{H}_{Tg} & = - * \begin{bmatrix} - * {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * @param state Pointer to state - * @param a_inI Linear acceleration with bias removed - */ - Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); - - /// Container for the noise values - ov_core::ImuConfig _imu_config; - - /// Our history of IMU messages (time, angular, linear) - std::vector imu_data; - - /// Gravity vector - Eigen::Vector3d _gravity; - }; +class Propagator { + +public: + /** + * @brief Default constructor + * @param noises imu noise characteristics (continuous time) + * @param gravity_mag Global gravity magnitude of the system (normally 9.81) + */ + Propagator(ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { + last_prop_time_offset = 0.0; + _gravity << 0.0, 0.0, gravity_mag; + } + + /** + * @brief Stores incoming inertial readings + * @param message Contains our timestamp and inertial information + */ + void feed_imu(const ov_core::ImuData &message) { + + // Append it to our vector + imu_data.emplace_back(message); + + // Loop through and delete imu messages that are older then 10 seconds + // TODO: we should probably have more elegant logic then this + // TODO: but this prevents unbounded memory growth and slow prop with high freq imu + auto it0 = imu_data.begin(); + while (it0 != imu_data.end()) { + if (message.timestamp - (*it0).timestamp > 10) { + it0 = imu_data.erase(it0); + } else { + it0++; + } + } + } + + /** + * @brief Propagate state up to given timestamp and then clone + * + * This will first collect all imu readings that occured between the + * *current* state time and the new time we want the state to be at. + * If we don't have any imu readings we will try to extrapolate into the future. + * After propagating the mean and covariance using our dynamics, + * We clone the current imu pose as a new clone in our state. + * + * @param state Pointer to state + * @param timestamp Time to propagate to and clone at + */ + void propagate_and_clone(std::shared_ptr state, double timestamp); + + /** + * @brief Gets what the state and its covariance will be at a given timestamp + * + * This can be used to find what the state will be in the "future" without propagating it. + * This will propagate a clone of the current IMU state and its covariance matrix. + * This is typically used to provide high frequency pose estimates between updates. + * + * @param state Pointer to state + * @param timestamp Time to propagate to + * @param state_plus The propagated state (q_GtoI, p_IinG, v_IinG, w_IinI) + */ + void fast_state_propagate(std::shared_ptr state, double timestamp, Eigen::Matrix &state_plus); + + /** + * @brief Helper function that given current imu data, will select imu readings between the two times. + * + * This will create measurements that we will integrate with, and an extra measurement at the end. + * We use the @ref interpolate_data() function to "cut" the imu readings at the begining and end of the integration. + * The timestamps passed should already take into account the time offset values. + * + * @param imu_data IMU data we will select measurements from + * @param time0 Start timestamp + * @param time1 End timestamp + * @param warn If we should warn if we don't have enough IMU to propagate with (e.g. fast prop will get warnings otherwise) + * @return Vector of measurements (if we could compute them) + */ + static std::vector select_imu_readings(const std::vector &imu_data, double time0, double time1, + bool warn = true); + + /** + * @brief Nice helper function that will linearly interpolate between two imu messages. + * + * This should be used instead of just "cutting" imu messages that bound the camera times + * Give better time offset if we use this function, could try other orders/splines if the imu is slow. + * + * @param imu_1 imu at begining of interpolation interval + * @param imu_2 imu at end of interpolation interval + * @param timestamp Timestamp being interpolated to + */ + static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) { + // time-distance lambda + double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp); + // PRINT_DEBUG("lambda - %d\n", lambda); + // interpolate between the two times + ov_core::ImuData data; + data.timestamp = timestamp; + data.am = (1 - lambda) * imu_1.am + lambda * imu_2.am; + data.wm = (1 - lambda) * imu_1.wm + lambda * imu_2.wm; + return data; + } + +protected: + /// Estimate for time offset at last propagation time + double last_prop_time_offset = 0.0; + bool have_last_prop_time_offset = false; + + /** + * @brief Propagates the state forward using the imu data and computes the noise covariance and state-transition + * matrix of this interval. + * + * This function can be replaced with analytical/numerical integration or when using a different state representation. + * This contains our state transition matrix along with how our noise evolves in time. + * If you have other state variables besides the IMU that evolve you would add them here. + * See the @ref error_prop page for details on how discrete model was derived. + * See the @ref imu_intrinsic page for details on how analytic model was derived. + * + * @param state Pointer to state + * @param data_minus imu readings at beginning of interval + * @param data_plus imu readings at end of interval + * @param F State-transition matrix over the interval + * @param Qd Discrete-time noise covariance over the interval + */ + void predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, + Eigen::MatrixXd &F, Eigen::MatrixXd &Qd); + + /** + * @brief Discrete imu mean propagation. + * + * See @ref propagation for details on these equations. + * \f{align*}{ + * \text{}^{I_{k+1}}_{G}\hat{\bar{q}} + * &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) + * \text{}^{I_{k}}_{G}\hat{\bar{q}} \\ + * ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t + * +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ + * ^G\hat{\mathbf{p}}_{I_{k+1}} + * &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t + * - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + * + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat1 Angular velocity with bias removed + * @param a_hat1 Linear acceleration with bias removed + * @param w_hat2 Next angular velocity with bias removed + * @param a_hat2 Next linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + */ + void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, + const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, + Eigen::Vector3d &new_p); + + /** + * @brief RK4 imu mean propagation. + * + * See this wikipedia page on [Runge-Kutta Methods](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods). + * We are doing a RK4 method, [this wolfram page](http://mathworld.wolfram.com/Runge-KuttaMethod.html) has the forth order equation + * defined below. We define function \f$ f(t,y) \f$ where y is a function of time t, see @ref imu_kinematic for the definition of the + * continous-time functions. + * + * \f{align*}{ + * {k_1} &= f({t_0}, y_0) \Delta t \\ + * {k_2} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_1} ) \Delta t \\ + * {k_3} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_2} ) \Delta t \\ + * {k_4} &= f( {t_0} + {\Delta t}, y_0 + {k_3} ) \Delta t \\ + * y_{0+\Delta t} &= y_0 + \left( {{1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4}} \right) + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat1 Angular velocity with bias removed + * @param a_hat1 Linear acceleration with bias removed + * @param w_hat2 Next angular velocity with bias removed + * @param a_hat2 Next linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + */ + void predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, + const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, + Eigen::Vector3d &new_p); + + /** + * @brief Analytically compute the integration components based on ACI^2 + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf), [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) + * + * For computing Xi_1, Xi_2, Xi_3 and Xi_4, you can refer to @ref imu_intrinsics or the following equations: + * \f{align*}{ + * \boldsymbol{\Xi}_1 & = \mathbf{I}_3 \delta t + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} \lfloor \hat{\mathbf{k}} \rfloor + * + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 + * \boldsymbol{\Xi}_2 & = \frac{1}{2} \delta t^2 \mathbf{I}_3 + + * \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor + * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 + * \boldsymbol{\Xi}_3 &= + * \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor + * + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} \lfloor\hat{\mathbf{a}} \rfloor \lfloor + * \hat{\mathbf{k}} \rfloor + * + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor\hat{\mathbf{a}} \rfloor + * \lfloor \hat{\mathbf{k}} \rfloor ^2 + * + \left( + * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \right) + * \lfloor \hat{\mathbf{k}} \rfloor ^2 \lfloor\hat{\mathbf{a}} \rfloor + * + \left( + * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} + * \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + * - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) + * }{\hat{\omega}^2} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 \boldsymbol{\Xi}_4 & = \frac{1}{6}\delta + * t^3 \lfloor\hat{\mathbf{a}} \rfloor + * + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} + * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + * + \left( + * \frac{2(1- \cos(\hat{\omega} \delta t)) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t)}{\hat{\omega}^3} + * \right) + * \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + * + \left( + * \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t}{\hat{\omega}^3} + + * \frac{\delta t^3}{6} + * \right) + * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + * + + * \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t + * \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} \lfloor \hat{\mathbf{k}} \rfloor^2\lfloor\hat{\mathbf{a}} \rfloor + * + + * \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t + * \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + * + + * \frac{4 \cos(\hat{\omega} \delta t) - 4 + (\hat{\omega} \delta t)^2 + \hat{\omega} \delta t \sin(\hat{\omega} \delta t) } + * {\hat{\omega}^3} + * \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 in order + */ + void compute_Xi_sum(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Matrix &Xi_sum); + + /** + * @brief Analytically predict IMU mean based on ACI^2 + * + * See the paper link: [RSS2020](https://yangyulin.net/papers/2020_rss.pdf), [ICRA2020](https://yangyulin.net/papers/2020_icra_aci.pdf) + * + * \f{align*}{ + * {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}^{\top}_k {}^{I_k}_G\hat{\mathbf{R}} \\ + * {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\delta t_k + + * {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{p}}_k - \frac{1}{2}{}^G\mathbf{g}\delta t^2_k \\ + * {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{v}}_k - + * {}^G\mathbf{g}\delta t_k + * \f} + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + */ + void predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, Eigen::Matrix &Xi_sum); + + /** + * @brief Analytically compute state transition matrix F and noise Jacobian G based on ACI^2 + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) + * + * This function is for analytical integration or when using a different state representation. + * This contains our state transition matrix and noise Jacobians. + * If you have other state variables besides the IMU that evolve you would add them here. + * See the @ref imu_intrinsics page for details on how this was derived. + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + * @param F State transition matrix + * @param G Noise Jacobian + */ + void compute_F_and_G_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, + const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, const Eigen::Matrix &Xi_sum, + Eigen::MatrixXd &F, Eigen::MatrixXd &G); + + /** + * @brief compute state transition matrix F and noise Jacobian G + * + * This function is for analytical integration or when using a different state representation. + * This contains our state transition matrix and noise Jacobians. + * If you have other state variables besides the IMU that evolve you would add them here. + * See the @ref imu_intrinsics page for details on how this was derived. + * + * @param state Pointer to state + * @param dt Time we should integrate over + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed + * @param w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed + * @param new_q The resulting new orientation after integration + * @param new_v The resulting new velocity after integration + * @param new_p The resulting new position after integration + * @param F State transition matrix + * @param G Noise Jacobian + */ + void compute_F_and_G_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, + const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, Eigen::MatrixXd &F, Eigen::MatrixXd &G); + + /** + * @brief compute the Jacobians for Dw + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * For kalibr model + * \f{align*}{ + * \mathbf{H}_{Dw} & = + * \begin{bmatrix} + * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 + * \end{bmatrix} + * \f} + * For rpng model: + * \f{align*}{ + * \mathbf{H}_{Dw} & = + * \begin{bmatrix} + * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * @param state Pointer to state + * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed + */ + Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); + + /** + * @brief compute the Jacobians for Da + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * + * For kalibr model + * \f{align*}{ + * \mathbf{H}_{Da} & = + * \begin{bmatrix} + * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * For rpng: + * \f{align*}{ + * \mathbf{H}_{Da} & = + * \begin{bmatrix} + * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 + * \end{bmatrix} + * \f} + * @param state Pointer to state + * @param a_uncorrected Linear acceleration in gyro frame with bias removed + */ + Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); + + /** + * @brief compute the Jacobians for Tg + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * + * + * \f{align*}{ + * \mathbf{H}_{Tg} & = + * \begin{bmatrix} + * {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * @param state Pointer to state + * @param a_inI Linear acceleration with bias removed + */ + Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); + + /// Container for the noise values + ov_core::ImuConfig _imu_config; + + /// Our history of IMU messages (time, angular, linear) + std::vector imu_data; + + /// Gravity vector + Eigen::Vector3d _gravity; +}; } // namespace ov_msckf diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 41d64622c..e3ebf05fe 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -134,15 +134,15 @@ class State { /// Accelerometer IMU intrinsics (scale imperfection and axis misalignment) std::shared_ptr _calib_imu_da; + /// Gyroscope gravity sensitivity + std::shared_ptr _calib_imu_tg; + /// Rotation from gyroscope frame to the "IMU" accelerometer frame std::shared_ptr _calib_imu_GYROtoIMU; /// Rotation from accelerometer to the "IMU" gyroscope frame frame std::shared_ptr _calib_imu_ACCtoIMU; - /// Gyroscope gravity sensitivity - std::shared_ptr _calib_imu_tg; - private: // Define that the state helper is a friend class of this class // This will allow it to access the below functions which should normally not be called diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 6fc891c60..920b8b90a 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -97,7 +97,7 @@ struct StateOptions { if (parser != nullptr) { parser->parse_config("use_fej", do_fej); parser->parse_config("use_imuavg", imu_avg); - parser->parse_config("use_rk4int", use_rk4_integration); + parser->parse_config("use_rk4_int", use_rk4_integration); parser->parse_config("use_analytic_int", use_analytic_integration); // Calibration booleans @@ -145,10 +145,14 @@ struct StateOptions { } PRINT_DEBUG(" - use_fej: %d\n", do_fej); PRINT_DEBUG(" - use_imuavg: %d\n", imu_avg); - PRINT_DEBUG(" - use_rk4int: %d\n", use_rk4_integration); + PRINT_DEBUG(" - use_rk4_int: %d\n", use_rk4_integration); + PRINT_DEBUG(" - use_analytic_int: %d\n", use_analytic_integration); PRINT_DEBUG(" - calib_cam_extrinsics: %d\n", do_calib_camera_pose); PRINT_DEBUG(" - calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); PRINT_DEBUG(" - calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); + PRINT_DEBUG(" - calib_imu_intrinsics: %d\n", do_calib_imu_intrinsics); + PRINT_DEBUG(" - calib_imu_g_sensitivity: %d\n", do_calib_imu_g_sensitivity); + PRINT_DEBUG(" - imu_model: %d\n", imu_model); PRINT_DEBUG(" - max_clones: %d\n", max_clone_size); PRINT_DEBUG(" - max_slam: %d\n", max_slam_features); PRINT_DEBUG(" - max_slam_in_update: %d\n", max_slam_in_update); @@ -158,6 +162,11 @@ struct StateOptions { PRINT_DEBUG(" - feat_rep_msckf: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); PRINT_DEBUG(" - feat_rep_slam: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_slam).c_str()); PRINT_DEBUG(" - feat_rep_aruco: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); + if ((do_calib_imu_intrinsics || do_calib_imu_g_sensitivity) && !use_analytic_integration) { + PRINT_ERROR(RED "calibrated IMU model selected, but not using analytical integration!\n" RESET); + PRINT_ERROR(RED "please enable analytical integration to perform this calibration!\n" RESET); + std::exit(EXIT_FAILURE); + } } }; diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 611219837..5189368bd 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -127,24 +127,24 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Measurement noise (convert from continuous to discrete) // Note the dt time might be different if we have "cut" any imu measurements - R.block(6 * i + 0, 6 * i + 0, 3, 3) *= _imu_config.sigma_w_2 / dt; + R.block(6 * i + 0, 6 * i + 0, 3, 3) *= std::pow(_imu_config.sigma_w, 2) / dt; if (!integrated_accel_constraint) { - R.block(6 * i + 3, 6 * i + 3, 3, 3) *= _imu_config.sigma_a_2 / dt; + R.block(6 * i + 3, 6 * i + 3, 3, 3) *= std::pow(_imu_config.sigma_a, 2) / dt; } else { - R.block(6 * i + 3, 6 * i + 3, 3, 3) *= _imu_config.sigma_a_2 * dt; + R.block(6 * i + 3, 6 * i + 3, 3, 3) *= std::pow(_imu_config.sigma_a, 2) * dt; } dt_summed += dt; } // Multiply our noise matrix by a fixed amount - // We typically need to treat the IMU as being "worst" to detect / not become over confident + // We typically need to treat the IMU as being "worst" to detect / not become overconfident R *= _zupt_noise_multiplier; // Next propagate the biases forward in time // NOTE: G*Qd*G^t = dt*Qd*dt = dt*Qc Eigen::MatrixXd Q_bias = Eigen::MatrixXd::Identity(6, 6); - Q_bias.block(0, 0, 3, 3) *= dt_summed * _imu_config.sigma_wb; - Q_bias.block(3, 3, 3, 3) *= dt_summed * _imu_config.sigma_ab; + Q_bias.block(0, 0, 3, 3) *= dt_summed * std::pow(_imu_config.sigma_wb, 2); + Q_bias.block(3, 3, 3, 3) *= dt_summed * std::pow(_imu_config.sigma_ab, 2); // Chi2 distance check // NOTE: we also append the propagation we "would do before the update" if this was to be accepted diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index 111b864d3..c4e72b36f 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -72,12 +72,6 @@ class UpdaterZeroVelocity { // Gravity _gravity << 0.0, 0.0, gravity_mag; - // Save our raw pixel noise squared - _imu_config.sigma_w_2 = std::pow(_imu_config.sigma_w, 2); - _imu_config.sigma_a_2 = std::pow(_imu_config.sigma_a, 2); - _imu_config.sigma_wb_2 = std::pow(_imu_config.sigma_wb, 2); - _imu_config.sigma_ab_2 = std::pow(_imu_config.sigma_ab, 2); - // Initialize the chi squared test table with confidence level 0.95 // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 for (int i = 1; i < 1000; i++) { From 29f17bd6e20565b59f201afbd7e24e3b09220fc8 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 14:31:03 -0500 Subject: [PATCH 20/60] small cleanup --- ov_msckf/src/state/Propagator.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 9d7bf067d..7002d3af3 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -164,18 +164,15 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times if (state->_options.imu_avg) { a_hat = .5 * (a_hat1 + a_hat2); } - // Imu acc readings in raw frame - Eigen::Vector3d a_uncorrected = a_hat; + // Correct imu readings with IMU intrinsics a_hat = state->R_AcctoI() * state->Da() * a_hat; a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; - // gravity sensitivity correction + // Corrected imu gyro measurements with our current biases Eigen::Vector3d gravity_correction1 = state->Tg() * a_hat1; Eigen::Vector3d gravity_correction2 = state->Tg() * a_hat2; - - // Corrected imu gyro measurements with our current biases Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; Eigen::Vector3d w_hat = w_hat1; From ba672394106f0fa65eea4176f1c5adbc1b58155a Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 16:37:11 -0500 Subject: [PATCH 21/60] more cleanup and formating --- ov_core/src/utils/sensor_data.h | 55 +- ov_msckf/src/core/VioManager.cpp | 2 +- ov_msckf/src/core/VioManagerOptions.h | 1 + ov_msckf/src/sim/Simulator.cpp | 42 +- ov_msckf/src/state/Propagator.cpp | 689 ++++++++++++-------------- ov_msckf/src/state/State.cpp | 55 +- ov_msckf/src/state/State.h | 72 ++- ov_msckf/src/state/StateOptions.h | 5 - 8 files changed, 424 insertions(+), 497 deletions(-) diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index 0975243ae..ad760ee78 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -86,6 +86,7 @@ struct CameraData { struct ImuConfig { /// imu model, 0: Kalibr model and 1: RPNG model + // TODO: convert this to an enum!!! int imu_model = 0; /// the columnwise elements for Dw @@ -97,10 +98,10 @@ struct ImuConfig { /// the ccolumnwise elements for Tg Eigen::Matrix vec_tg; - /// the JPL quat for R_AcctoI + /// the JPL quat for R_ACCtoIMU Eigen::Matrix q_ACCtoIMU; - /// the JPL quat for R_GyrotoI + /// the JPL quat for R_GYROtoIMU Eigen::Matrix q_GYROtoIMU; /// Gyroscope white noise (rad/s/sqrt(hz)) @@ -122,56 +123,6 @@ struct ImuConfig { PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); } - - /// get the IMU Dw - Eigen::Matrix3d Dw() { - Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if (imu_model == 0) { - // Kalibr model with lower triangular of the matrix - Dw << vec_dw(0), 0, 0, vec_dw(1), vec_dw(3), 0, vec_dw(2), vec_dw(4), vec_dw(5); - } else { - // rpng model with upper triangular of the matrix - Dw << vec_dw(0), vec_dw(1), vec_dw(3), 0, vec_dw(2), vec_dw(4), 0, 0, vec_dw(5); - } - return Dw; - } - - /// get the IMU Da - Eigen::Matrix3d Da() { - Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (imu_model == 0) { - // kalibr model with lower triangular of the matrix - Da << vec_da(0), 0, 0, vec_da(1), vec_da(3), 0, vec_da(2), vec_da(4), vec_da(5); - } else { - // rpng model with upper triangular of the matrix - Da << vec_da(0), vec_da(1), vec_da(3), 0, vec_da(2), vec_da(4), 0, 0, vec_da(5); - } - return Da; - } - - /// get the IMU Tw - Eigen::Matrix3d Tw() { return Dw().inverse(); } - - /// get the IMU Ta - Eigen::Matrix3d Ta() { return Da().inverse(); } - - /// get the IMU Tg - Eigen::Matrix3d Tg() { - Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << vec_tg(0), vec_tg(3), vec_tg(6), vec_tg(1), vec_tg(4), vec_tg(7), vec_tg(2), vec_tg(5), vec_tg(8); - return Tg; - } - - /// get the R_AcctoI - Eigen::Matrix3d R_AcctoI() { return ov_core::quat_2_Rot(q_ACCtoIMU); } - /// get the R_ItoAcc - Eigen::Matrix3d R_ItoAcc() { return R_AcctoI().transpose(); } - - /// get the R_GyrotoI - Eigen::Matrix3d R_GyrotoI() { return ov_core::quat_2_Rot(q_GYROtoIMU); } - - /// get the R_ItoGyro - Eigen::Matrix3d R_ItoGyro() { return R_GyrotoI().transpose(); } }; } // namespace ov_core diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 9d907051b..56334dd81 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -741,7 +741,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) state->_calib_imu_da->value()(5)); } if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { - PRINT_INFO("Tg = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_tg->value()(0), + PRINT_INFO("Tg = | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_tg->value()(0), state->_calib_imu_tg->value()(1), state->_calib_imu_tg->value()(2), state->_calib_imu_tg->value()(3), state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), state->_calib_imu_tg->value()(6), state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 5ab704257..c430dcb81 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -355,6 +355,7 @@ struct VioManagerOptions { } PRINT_DEBUG("IMU PARAMETERS:\n"); std::stringstream ss; + ss << "imu model:" << ((imu_config.imu_model == 0) ? "kalibr" : "rpng") << std::endl; ss << "Dw (columnwise):" << imu_config.vec_dw.transpose() << std::endl; ss << "Da (columnwise):" << imu_config.vec_da.transpose() << std::endl; ss << "Tg (columnwise):" << imu_config.vec_tg.transpose() << std::endl; diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 58a1184d8..0f8930f5b 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -330,22 +330,42 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto Eigen::Vector3d accel_inI = R_GtoI * (a_IinG + gravity); Eigen::Vector3d omega_inI = w_IinI; + // Get our imu intrinsic parameters + // - kalibr: lower triangular of the matrix is used + // - rpng: upper triangular of the matrix is used + Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); + Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); + if (params.imu_config.imu_model == 0) { + Dw << params.imu_config.vec_dw(0), 0, 0, params.imu_config.vec_dw(1), params.imu_config.vec_dw(3), 0, params.imu_config.vec_dw(2), + params.imu_config.vec_dw(4), params.imu_config.vec_dw(5); + Da << params.imu_config.vec_da(0), 0, 0, params.imu_config.vec_da(1), params.imu_config.vec_da(3), 0, params.imu_config.vec_da(2), + params.imu_config.vec_da(4), params.imu_config.vec_da(5); + } else { + Dw << params.imu_config.vec_dw(0), params.imu_config.vec_dw(1), params.imu_config.vec_dw(3), 0, params.imu_config.vec_dw(2), + params.imu_config.vec_dw(4), 0, 0, params.imu_config.vec_dw(5); + Da << params.imu_config.vec_da(0), params.imu_config.vec_da(1), params.imu_config.vec_da(3), 0, params.imu_config.vec_da(2), + params.imu_config.vec_da(4), 0, 0, params.imu_config.vec_da(5); + } + Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); + Tg << params.imu_config.vec_tg(0), params.imu_config.vec_tg(3), params.imu_config.vec_tg(6), params.imu_config.vec_tg(1), + params.imu_config.vec_tg(4), params.imu_config.vec_tg(7), params.imu_config.vec_tg(2), params.imu_config.vec_tg(5), + params.imu_config.vec_tg(8); + // Get the readings with the imu intrinsic "distortion" - Eigen::Matrix3d Tg = params.imu_config.Tg(); - Eigen::Matrix3d Tw = params.imu_config.Tw(); - Eigen::Matrix3d Ta = params.imu_config.Ta(); - Eigen::Vector3d omega_inw = Tw * params.imu_config.R_ItoGyro() * omega_inI + Tg * accel_inI; - Eigen::Vector3d accel_ina = Ta * params.imu_config.R_ItoAcc() * accel_inI; + Eigen::Matrix3d Tw = Dw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d Ta = Da.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Vector3d omega_inGYRO = Tw * quat_2_Rot(params.imu_config.q_GYROtoIMU).transpose() * omega_inI + Tg * accel_inI; + Eigen::Vector3d accel_inACC = Ta * quat_2_Rot(params.imu_config.q_ACCtoIMU).transpose() * accel_inI; // Now add noise to these measurements double dt = 1.0 / params.sim_freq_imu; std::normal_distribution w(0, 1); - wm(0) = omega_inw(0) + true_bias_gyro(0) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(1) = omega_inw(1) + true_bias_gyro(1) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(2) = omega_inw(2) + true_bias_gyro(2) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - am(0) = accel_ina(0) + true_bias_accel(0) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(1) = accel_ina(1) + true_bias_accel(1) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(2) = accel_ina(2) + true_bias_accel(2) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + wm(0) = omega_inGYRO(0) + true_bias_gyro(0) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(1) = omega_inGYRO(1) + true_bias_gyro(1) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(2) = omega_inGYRO(2) + true_bias_gyro(2) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + am(0) = accel_inACC(0) + true_bias_accel(0) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(1) = accel_inACC(1) + true_bias_accel(1) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(2) = accel_inACC(2) + true_bias_accel(2) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); // Move the biases forward in time true_bias_gyro(0) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 7002d3af3..1aab9578c 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -95,10 +95,11 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest Eigen::Vector3d last_a = Eigen::Vector3d::Zero(); Eigen::Vector3d last_w = Eigen::Vector3d::Zero(); if (!prop_data.empty()) { - last_a = state->R_AcctoI() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); + last_a = state->_calib_imu_ACCtoIMU->Rot() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); // TODO: is this acceleration used correct? // Eigen::Vector3d last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; - last_w = state->R_GyrotoI() * state->Dw() * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); + last_w = state->_calib_imu_GYROtoIMU->Rot() * state->Dw() * + (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); } // Do the update to the covariance with our "summed" state transition and IMU noise addition... @@ -166,24 +167,24 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times } // Correct imu readings with IMU intrinsics - a_hat = state->R_AcctoI() * state->Da() * a_hat; - a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; - a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; + Eigen::Matrix3d R_ACCtoIMU = state->_calib_imu_ACCtoIMU->Rot(); + a_hat = R_ACCtoIMU * state->Da() * a_hat; + a_hat1 = R_ACCtoIMU * state->Da() * a_hat1; + a_hat2 = R_ACCtoIMU * state->Da() * a_hat2; // Corrected imu gyro measurements with our current biases - Eigen::Vector3d gravity_correction1 = state->Tg() * a_hat1; - Eigen::Vector3d gravity_correction2 = state->Tg() * a_hat2; - Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; - Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; + Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - state->Tg() * a_hat1; + Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - state->Tg() * a_hat2; Eigen::Vector3d w_hat = w_hat1; if (state->_options.imu_avg) { w_hat = .5 * (w_hat1 + w_hat2); } // Correct imu readings with IMU intrinsics - w_hat = state->R_GyrotoI() * state->Dw() * w_hat; - w_hat1 = state->R_GyrotoI() * state->Dw() * w_hat1; - w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; + Eigen::Matrix3d R_GYROtoIMU = state->_calib_imu_GYROtoIMU->Rot(); + w_hat = R_GYROtoIMU * state->Dw() * w_hat; + w_hat1 = R_GYROtoIMU * state->Dw() * w_hat1; + w_hat2 = R_GYROtoIMU * state->Dw() * w_hat2; // Pre-compute some analytical values Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); @@ -213,7 +214,7 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times } // Now record what the predicted state should be - // TODO: apply IMU intrinsics here to correct the angular velocity + // TODO: apply IMU intrinsics here to correct the returned angular velocity state_plus = Eigen::Matrix::Zero(); state_plus.block(0, 0, 4, 1) = state->_imu->quat(); state_plus.block(4, 0, 3, 1) = state->_imu->pos(); @@ -349,10 +350,6 @@ std::vector Propagator::select_imu_readings(const std::vector< void Propagator::predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, Eigen::MatrixXd &F, Eigen::MatrixXd &Qd) { - // Set them to zero - F.setZero(); - Qd.setZero(); - // Time elapsed over interval double dt = data_plus.timestamp - data_minus.timestamp; // assert(data_plus.timestamp>data_minus.timestamp); @@ -367,15 +364,14 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Convert "raw" imu to its corrected frame using the IMU intrinsics Eigen::Vector3d a_uncorrected = a_hat; - a_hat = state->R_AcctoI() * state->Da() * a_hat; - a_hat1 = state->R_AcctoI() * state->Da() * a_hat1; - a_hat2 = state->R_AcctoI() * state->Da() * a_hat2; - - // Corrected imu gyro measurements with our current biases and gravity sensativity - Eigen::Vector3d gravity_correction1 = state->Tg() * a_hat1; - Eigen::Vector3d gravity_correction2 = state->Tg() * a_hat2; - Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - gravity_correction1; - Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - gravity_correction2; + Eigen::Matrix3d R_ACCtoIMU = state->_calib_imu_ACCtoIMU->Rot(); + a_hat = R_ACCtoIMU * state->Da() * a_hat; + a_hat1 = R_ACCtoIMU * state->Da() * a_hat1; + a_hat2 = R_ACCtoIMU * state->Da() * a_hat2; + + // Corrected imu gyro measurements with our current biases and gravity sensitivity + Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - state->Tg() * a_hat1; + Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - state->Tg() * a_hat2; Eigen::Vector3d w_hat = w_hat1; if (state->_options.imu_avg) { w_hat = .5 * (w_hat1 + w_hat2); @@ -383,9 +379,10 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Convert "raw" imu to its corrected frame using the IMU intrinsics Eigen::Vector3d w_uncorrected = w_hat; - w_hat = state->R_GyrotoI() * state->Dw() * w_hat; - w_hat1 = state->R_GyrotoI() * state->Dw() * w_hat1; - w_hat2 = state->R_GyrotoI() * state->Dw() * w_hat2; + Eigen::Matrix3d R_GYROtoIMU = state->_calib_imu_GYROtoIMU->Rot(); + w_hat = R_GYROtoIMU * state->Dw() * w_hat; + w_hat1 = R_GYROtoIMU * state->Dw() * w_hat1; + w_hat2 = R_GYROtoIMU * state->Dw() * w_hat2; // Pre-compute some analytical values for the mean and covariance integration Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); @@ -436,6 +433,222 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core state->_imu->set_fej(imu_x); } +void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, + const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { + + // If we are averaging the IMU, then do so + Eigen::Vector3d w_hat = w_hat1; + Eigen::Vector3d a_hat = a_hat1; + if (state->_options.imu_avg) { + w_hat = .5 * (w_hat1 + w_hat2); + a_hat = .5 * (a_hat1 + a_hat2); + } + + // Pre-compute things + double w_norm = w_hat.norm(); + Eigen::Matrix I_4x4 = Eigen::Matrix::Identity(); + Eigen::Matrix3d R_Gtoi = state->_imu->Rot(); + + // Orientation: Equation (101) and (103) and of Trawny indirect TR + Eigen::Matrix bigO; + if (w_norm > 1e-12) { + bigO = cos(0.5 * w_norm * dt) * I_4x4 + 1 / w_norm * sin(0.5 * w_norm * dt) * Omega(w_hat); + } else { + bigO = I_4x4 + 0.5 * dt * Omega(w_hat); + } + new_q = quatnorm(bigO * state->_imu->quat()); + // new_q = rot_2_quat(exp_so3(-w_hat*dt)*R_Gtoi); + + // Velocity: just the acceleration in the local frame, minus global gravity + new_v = state->_imu->vel() + R_Gtoi.transpose() * a_hat * dt - _gravity * dt; + + // Position: just velocity times dt, with the acceleration integrated twice + new_p = state->_imu->pos() + state->_imu->vel() * dt + 0.5 * R_Gtoi.transpose() * a_hat * dt * dt - 0.5 * _gravity * dt * dt; +} + +void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, + const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, + Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { + + // Pre-compute things + Eigen::Vector3d w_hat = w_hat1; + Eigen::Vector3d a_hat = a_hat1; + Eigen::Vector3d w_alpha = (w_hat2 - w_hat1) / dt; + Eigen::Vector3d a_jerk = (a_hat2 - a_hat1) / dt; + + // y0 ================ + Eigen::Vector4d q_0 = state->_imu->quat(); + Eigen::Vector3d p_0 = state->_imu->pos(); + Eigen::Vector3d v_0 = state->_imu->vel(); + + // k1 ================ + Eigen::Vector4d dq_0 = {0, 0, 0, 1}; + Eigen::Vector4d q0_dot = 0.5 * Omega(w_hat) * dq_0; + Eigen::Vector3d p0_dot = v_0; + Eigen::Matrix3d R_Gto0 = quat_2_Rot(quat_multiply(dq_0, q_0)); + Eigen::Vector3d v0_dot = R_Gto0.transpose() * a_hat - _gravity; + + Eigen::Vector4d k1_q = q0_dot * dt; + Eigen::Vector3d k1_p = p0_dot * dt; + Eigen::Vector3d k1_v = v0_dot * dt; + + // k2 ================ + w_hat += 0.5 * w_alpha * dt; + a_hat += 0.5 * a_jerk * dt; + + Eigen::Vector4d dq_1 = quatnorm(dq_0 + 0.5 * k1_q); + // Eigen::Vector3d p_1 = p_0+0.5*k1_p; + Eigen::Vector3d v_1 = v_0 + 0.5 * k1_v; + + Eigen::Vector4d q1_dot = 0.5 * Omega(w_hat) * dq_1; + Eigen::Vector3d p1_dot = v_1; + Eigen::Matrix3d R_Gto1 = quat_2_Rot(quat_multiply(dq_1, q_0)); + Eigen::Vector3d v1_dot = R_Gto1.transpose() * a_hat - _gravity; + + Eigen::Vector4d k2_q = q1_dot * dt; + Eigen::Vector3d k2_p = p1_dot * dt; + Eigen::Vector3d k2_v = v1_dot * dt; + + // k3 ================ + Eigen::Vector4d dq_2 = quatnorm(dq_0 + 0.5 * k2_q); + // Eigen::Vector3d p_2 = p_0+0.5*k2_p; + Eigen::Vector3d v_2 = v_0 + 0.5 * k2_v; + + Eigen::Vector4d q2_dot = 0.5 * Omega(w_hat) * dq_2; + Eigen::Vector3d p2_dot = v_2; + Eigen::Matrix3d R_Gto2 = quat_2_Rot(quat_multiply(dq_2, q_0)); + Eigen::Vector3d v2_dot = R_Gto2.transpose() * a_hat - _gravity; + + Eigen::Vector4d k3_q = q2_dot * dt; + Eigen::Vector3d k3_p = p2_dot * dt; + Eigen::Vector3d k3_v = v2_dot * dt; + + // k4 ================ + w_hat += 0.5 * w_alpha * dt; + a_hat += 0.5 * a_jerk * dt; + + Eigen::Vector4d dq_3 = quatnorm(dq_0 + k3_q); + // Eigen::Vector3d p_3 = p_0+k3_p; + Eigen::Vector3d v_3 = v_0 + k3_v; + + Eigen::Vector4d q3_dot = 0.5 * Omega(w_hat) * dq_3; + Eigen::Vector3d p3_dot = v_3; + Eigen::Matrix3d R_Gto3 = quat_2_Rot(quat_multiply(dq_3, q_0)); + Eigen::Vector3d v3_dot = R_Gto3.transpose() * a_hat - _gravity; + + Eigen::Vector4d k4_q = q3_dot * dt; + Eigen::Vector3d k4_p = p3_dot * dt; + Eigen::Vector3d k4_v = v3_dot * dt; + + // y+dt ================ + Eigen::Vector4d dq = quatnorm(dq_0 + (1.0 / 6.0) * k1_q + (1.0 / 3.0) * k2_q + (1.0 / 3.0) * k3_q + (1.0 / 6.0) * k4_q); + new_q = quat_multiply(dq, q_0); + new_p = p_0 + (1.0 / 6.0) * k1_p + (1.0 / 3.0) * k2_p + (1.0 / 3.0) * k3_p + (1.0 / 6.0) * k4_p; + new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v; +} + +void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Matrix &Xi_sum) { + + // Decompose our angular velocity into a direction and amount + double w_norm = w_hat.norm(); + double d_th = w_norm * d_t; + Eigen::Vector3d k_hat; + if (w_norm < 1e-8) { + k_hat << 0, 0, 0; + } else { + k_hat << w_hat / w_norm; + } + + // Compute useful identities used throughout + Eigen::Matrix3d I_3x3 = Eigen::Matrix3d::Identity(); + double d_t2 = std::pow(d_t, 2); + double d_t3 = std::pow(d_t, 3); + double w_norm2 = std::pow(w_norm, 2); + double w_norm3 = std::pow(w_norm, 3); + double cos_dth = std::cos(d_th); + double sin_dth = std::sin(d_th); + double d_th2 = std::pow(d_th, 2); + double d_th3 = std::pow(d_th, 3); + Eigen::Matrix3d sK = ov_core::skew_x(k_hat); + Eigen::Matrix3d sK2 = sK * sK; + Eigen::Matrix3d sA = ov_core::skew_x(a_hat); + + // Based on the delta theta, let's decide which integration will be used + bool small_w = (w_norm < 1.0 / 180 * M_PI / 2); + bool small_th = (d_th < 1.0 / 180 * M_PI / 2); + + // Integration components will be used later + Eigen::Matrix3d R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; + + // Now the R and its jacobian J can be computed + R_k1tok = I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2; + if (!small_th) { + Jr_k1tok = I_3x3 - (1.0 - cos_dth) / d_th * sK + (d_th - sin_dth) / d_th * sK2; + } else { + Jr_k1tok = I_3x3 - sin_dth * sK + (1.0 - cos_dth) * sK2; + } + + // Now begin the integration of each component + if (!small_w) { + + // first order rotation integration with constant omega + Xi_1 = I_3x3 * d_t + (1.0 - cos_dth) / w_norm * sK + (d_t - sin_dth / w_norm) * sK2; + + // second order rotation integration with constat omega + Xi_2 = 1.0 / 2 * d_t2 * I_3x3 + (d_th - sin_dth) / w_norm2 * sK + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sK2; + + // first order RAJ integratioin with constant omega and constant acc + Xi_3 = 1.0 / 2 * d_t2 * sA + (sin_dth - d_th) / w_norm2 * sA * sK + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA + + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sA * sK2 + + (1.0 / 2 * d_t2 - (1.0 - cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - + (3 * sin_dth - 2 * d_th - d_th * cos_dth) / w_norm2 * k_hat.dot(a_hat) * sK2; + + // second order RAJ integration with constant omega and constant acc + Xi_4 = 1.0 / 6 * d_t3 * sA + (2 * (1.0 - cos_dth) - d_th2) / (2 * w_norm3) * sA * sK + + ((2 * (1.0 - cos_dth) - d_th * sin_dth) / w_norm3) * sK * sA + ((sin_dth - d_th) / w_norm3 + d_t3 / 6) * sA * sK2 + + ((d_th - 2 * sin_dth + 1.0 / 6 * d_th3 + d_th * cos_dth) / w_norm3) * (sK2 * sA + k_hat.dot(a_hat) * sK) + + (4 * cos_dth - 4 + d_th2 + d_th * sin_dth) / w_norm3 * k_hat.dot(a_hat) * sK2; + + } else { + + // first order rotation + Xi_1 = d_t * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); + + // second order rotation + Xi_2 = 1.0 / 2 * d_t * Xi_1; + // iint_R = 1.0/2 * d_t2 * (I_3x3 + sin_dth * sK + (1.0-cos_dth) * sK2); + + // first order RAJ + Xi_3 = 1.0 / 2 * d_t2 * + (sA + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); + + // second order RAJ + Xi_4 = 1.0 / 3 * d_t * Xi_3; + } + + // Store the integrated parameters + Xi_sum << R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; +} + +void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, + Eigen::Matrix &Xi_sum) { + + // Pre-compute things + Eigen::Matrix3d R_Gtok = state->_imu->Rot(); + Eigen::Matrix3d R_k1_to_k = Xi_sum.block(0, 0, 3, 3); + Eigen::Vector4d q_k_to_k1 = ov_core::rot_2_quat(R_k1_to_k.transpose()); + Eigen::Matrix3d Xi_1 = Xi_sum.block(0, 3, 3, 3); + Eigen::Matrix3d Xi_2 = Xi_sum.block(0, 6, 3, 3); + + // Use our integrated Xi's to move the state forward + new_q = ov_core::quat_multiply(q_k_to_k1, state->_imu->quat()); + new_v = state->_imu->vel() + R_Gtok.transpose() * Xi_1 * a_hat - _gravity * dt; + new_p = state->_imu->pos() + state->_imu->vel() * dt + R_Gtok.transpose() * Xi_2 * a_hat - 0.5 * _gravity * dt * dt; +} + void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, @@ -470,7 +683,6 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Tg_id = local_size; local_size += state->_calib_imu_tg->size(); } - // 0: kalibr, 1: rpng if (state->_options.imu_model == 0) { th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); @@ -495,8 +707,8 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Eigen::Matrix3d Da = state->Da(); Eigen::Matrix3d Dw = state->Dw(); Eigen::Matrix3d Tg = state->Tg(); - Eigen::Matrix3d R_atoI = state->R_AcctoI(); - Eigen::Matrix3d R_wtoI = state->R_GyrotoI(); + Eigen::Matrix3d R_atoI = state->_calib_imu_ACCtoIMU->Rot(); + Eigen::Matrix3d R_wtoI = state->_calib_imu_GYROtoIMU->Rot(); Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains gravity correction already @@ -507,28 +719,28 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Eigen::Matrix3d Xi_4 = Xi_sum.block<3, 3>(0, 15); // for th - F.block<3, 3>(th_id, th_id) = dR_ktok1; - F.block<3, 3>(p_id, th_id).noalias() = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); - F.block<3, 3>(v_id, th_id).noalias() = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); + F.block(th_id, th_id, 3, 3) = dR_ktok1; + F.block(p_id, th_id, 3, 3) = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); + F.block(v_id, th_id, 3, 3) = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); // for p - F.block<3, 3>(p_id, p_id).setIdentity(); + F.block<3, 3>(p_id, p_id, 3, 3).setIdentity(); // for v - F.block<3, 3>(p_id, v_id) = Eigen::Matrix3d::Identity() * dt; - F.block<3, 3>(v_id, v_id).setIdentity(); + F.block<3, 3>(p_id, v_id, 3, 3) = Eigen::Matrix3d::Identity() * dt; + F.block<3, 3>(v_id, v_id, 3, 3).setIdentity(); // for bg - F.block<3, 3>(th_id, bg_id).noalias() = -Jr * dt * R_wtoI * Dw; - F.block<3, 3>(p_id, bg_id) = R_k.transpose() * Xi_4 * R_wtoI * Dw; - F.block<3, 3>(v_id, bg_id) = R_k.transpose() * Xi_3 * R_wtoI * Dw; - F.block<3, 3>(bg_id, bg_id).setIdentity(); + F.block<3, 3>(th_id, bg_id, 3, 3) = -Jr * dt * R_wtoI * Dw; + F.block<3, 3>(p_id, bg_id, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI * Dw; + F.block<3, 3>(v_id, bg_id, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI * Dw; + F.block<3, 3>(bg_id, bg_id, 3, 3).setIdentity(); // for ba - F.block<3, 3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - F.block<3, 3>(p_id, ba_id) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; - F.block<3, 3>(v_id, ba_id) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; - F.block<3, 3>(ba_id, ba_id).setIdentity(); + F.block(th_id, ba_id, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block(p_id, ba_id, 3, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; + F.block(v_id, ba_id, 3, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; + F.block(ba_id, ba_id, 3, 3).setIdentity(); // begin to add the state transition matrix for the omega intrinsics part if (Dw_id != -1) { @@ -558,29 +770,29 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // begin to add the state transition matrix for the acctoI part if (th_atoI_id != -1) { - F.block<3, 3>(th_atoI_id, th_atoI_id).setIdentity(); - F.block<3, 3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); - F.block<3, 3>(p_id, th_atoI_id) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); - F.block<3, 3>(v_id, th_atoI_id) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); + F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); + F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block(p_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); + F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); } // begin to add the state transition matrix for the gyrotoI part if (th_wtoI_id != -1) { - F.block<3, 3>(th_wtoI_id, th_wtoI_id).setIdentity(); - F.block<3, 3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); - F.block<3, 3>(p_id, th_wtoI_id) = -R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); - F.block<3, 3>(v_id, th_wtoI_id) = -R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); + F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); + F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); + F.block(p_id, th_wtoI_id, 3, 3) = -R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); + F.block(v_id, th_wtoI_id, 3, 3) = -R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); } // construct the G part - G.block<3, 3>(th_id, 0) = -Jr * dt * R_wtoI * Dw; - G.block<3, 3>(p_id, 0) = R_k.transpose() * Xi_4 * R_wtoI * Dw; - G.block<3, 3>(v_id, 0) = R_k.transpose() * Xi_3 * R_wtoI * Dw; - G.block<3, 3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - G.block<3, 3>(p_id, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; - G.block<3, 3>(v_id, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; - G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix3d::Identity(); - G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix3d::Identity(); + G.block(th_id, 0, 3, 3) = -Jr * dt * R_wtoI * Dw; + G.block(p_id, 0, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI * Dw; + G.block(v_id, 0, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI * Dw; + G.block(th_id, 3, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block(p_id, 3, 3, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; + G.block(v_id, 3, 3, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; + G.block(bg_id, 6, 3, 3) = dt * Eigen::Matrix3d::Identity(); + G.block(ba_id, 9, 3, 3) = dt * Eigen::Matrix3d::Identity(); } void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, @@ -617,25 +829,19 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d local_size += state->_calib_imu_tg->size(); } if (state->_options.imu_model == 0) { - // Kalibr model th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); } else { - // RPNG model th_atoI_id = local_size; local_size += state->_calib_imu_ACCtoIMU->size(); } } - //============================================================ - //============================================================ - // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information Eigen::Matrix3d R_k = state->_imu->Rot(); Eigen::Vector3d v_k = state->_imu->vel(); Eigen::Vector3d p_k = state->_imu->pos(); - if (state->_options.do_fej) { R_k = state->_imu->Rot_fej(); v_k = state->_imu->vel_fej(); @@ -648,33 +854,34 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d Eigen::Matrix3d Da = state->Da(); Eigen::Matrix3d Dw = state->Dw(); Eigen::Matrix3d Tg = state->Tg(); - Eigen::Matrix3d R_atoI = state->R_AcctoI(); - Eigen::Matrix3d R_wtoI = state->R_GyrotoI(); + Eigen::Matrix3d R_atoI = state->_calib_imu_ACCtoIMU->Rot(); + Eigen::Matrix3d R_wtoI = state->_calib_imu_GYROtoIMU->Rot(); Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; - Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains the gravity correction already + Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains gravity correction already Eigen::Matrix3d Jr = Jr_so3(w_k * dt); // for theta - F.block<3, 3>(th_id, th_id) = dR_ktok1; - // F.block(th_id, bg_id, 3, 3).noalias() = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; - F.block<3, 3>(th_id, bg_id).noalias() = -Jr * dt * R_wtoI * Dw; - F.block<3, 3>(th_id, ba_id).noalias() = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block(th_id, th_id, 3, 3) = dR_ktok1; + // F.block(th_id, bg_id, 3, 3) = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; + F.block(th_id, bg_id, 3, 3) = -Jr * dt * R_wtoI * Dw; + F.block(th_id, ba_id, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; // for position - F.block<3, 3>(p_id, th_id).noalias() = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); - F.block<3, 3>(p_id, p_id).setIdentity(); - F.block<3, 3>(p_id, v_id) = Eigen::Matrix3d::Identity() * dt; - F.block<3, 3>(p_id, ba_id) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + F.block(p_id, th_id, 3, 3) = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); + F.block(p_id, p_id, 3, 3).setIdentity(); + F.block(p_id, v_id, 3, 3) = Eigen::Matrix3d::Identity() * dt; + F.block(p_id, ba_id, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; // for velocity - F.block<3, 3>(v_id, th_id).noalias() = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); - F.block<3, 3>(v_id, v_id).setIdentity(); - F.block<3, 3>(v_id, ba_id) = -R_k.transpose() * dt * R_atoI * Da; + F.block(v_id, th_id, 3, 3) = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); + F.block(v_id, v_id, 3, 3).setIdentity(); + F.block(v_id, ba_id, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; // for bg - F.block<3, 3>(bg_id, bg_id).setIdentity(); + F.block(bg_id, bg_id, 3, 3).setIdentity(); + // for ba - F.block<3, 3>(ba_id, ba_id).setIdentity(); + F.block(ba_id, ba_id, 3, 3).setIdentity(); // begin to add the state transition matrix for the omega intrinsics part if (Dw_id != -1) { @@ -692,16 +899,16 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the acc intrinsics part if (th_atoI_id != -1) { - F.block<3, 3>(th_atoI_id, th_atoI_id).setIdentity(); - F.block<3, 3>(th_id, th_atoI_id) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); - F.block<3, 3>(p_id, th_atoI_id) = 0.5 * R_k.transpose() * dt * dt * ov_core::skew_x(a_k); - F.block<3, 3>(v_id, th_atoI_id) = R_k.transpose() * dt * ov_core::skew_x(a_k); + F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); + F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block(p_id, th_atoI_id, 3, 3) = 0.5 * R_k.transpose() * dt * dt * ov_core::skew_x(a_k); + F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * dt * ov_core::skew_x(a_k); } // begin to add the state transition matrix for the gyro intrinsics part if (th_wtoI_id != -1) { - F.block<3, 3>(th_wtoI_id, th_wtoI_id).setIdentity(); - F.block<3, 3>(th_id, th_wtoI_id) = Jr * dt * ov_core::skew_x(w_k); + F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); + F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); } // begin to add the state transition matrix for the gravity sensitivity part @@ -711,296 +918,51 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d } // Noise jacobian - G.block<3, 3>(th_id, 0) = -Jr * dt * R_wtoI * Dw; - G.block<3, 3>(th_id, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; - G.block<3, 3>(v_id, 3) = -R_k.transpose() * dt * R_atoI * Da; - G.block<3, 3>(p_id, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; - G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix3d::Identity(); - G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix3d::Identity(); -} - -void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, - const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, - Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { - - // If we are averaging the IMU, then do so - Eigen::Vector3d w_hat = w_hat1; - Eigen::Vector3d a_hat = a_hat1; - if (state->_options.imu_avg) { - w_hat = .5 * (w_hat1 + w_hat2); - a_hat = .5 * (a_hat1 + a_hat2); - } - - // Pre-compute things - double w_norm = w_hat.norm(); - Eigen::Matrix I_4x4 = Eigen::Matrix::Identity(); - Eigen::Matrix3d R_Gtoi = state->_imu->Rot(); - - // Orientation: Equation (101) and (103) and of Trawny indirect TR - Eigen::Matrix bigO; - if (w_norm > 1e-12) { - bigO = cos(0.5 * w_norm * dt) * I_4x4 + 1 / w_norm * sin(0.5 * w_norm * dt) * Omega(w_hat); - } else { - bigO = I_4x4 + 0.5 * dt * Omega(w_hat); - } - new_q = quatnorm(bigO * state->_imu->quat()); - // new_q = rot_2_quat(exp_so3(-w_hat*dt)*R_Gtoi); - - // Velocity: just the acceleration in the local frame, minus global gravity - new_v = state->_imu->vel() + R_Gtoi.transpose() * a_hat * dt - _gravity * dt; - - // Position: just velocity times dt, with the acceleration integrated twice - new_p = state->_imu->pos() + state->_imu->vel() * dt + 0.5 * R_Gtoi.transpose() * a_hat * dt * dt - 0.5 * _gravity * dt * dt; -} - -void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, - Eigen::Matrix &Xi_sum) { - - // Pre-compute things - Eigen::Matrix3d R_Gtok = state->_imu->Rot(); - - // get the pre-computed value - Eigen::Matrix3d R_k1_to_k = Xi_sum.block<3, 3>(0, 0); - Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3); - Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6); - - // use the precomputed value to get the new state - Eigen::Matrix q_k_to_k1 = ov_core::rot_2_quat(R_k1_to_k.transpose()); - new_q = ov_core::quat_multiply(q_k_to_k1, state->_imu->quat()); - - // Velocity: just the acceleration in the local frame, minus global gravity - new_v = state->_imu->vel() + R_Gtok.transpose() * Xi_1 * a_hat - _gravity * dt; - - // Position: just velocity times dt, with the acceleration integrated twice - new_p = state->_imu->pos() + state->_imu->vel() * dt + R_Gtok.transpose() * Xi_2 * a_hat - 0.5 * _gravity * dt * dt; -} - -void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, - const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, - Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { - - // Pre-compute things - Eigen::Vector3d w_hat = w_hat1; - Eigen::Vector3d a_hat = a_hat1; - Eigen::Vector3d w_alpha = (w_hat2 - w_hat1) / dt; - Eigen::Vector3d a_jerk = (a_hat2 - a_hat1) / dt; - - // y0 ================ - Eigen::Vector4d q_0 = state->_imu->quat(); - Eigen::Vector3d p_0 = state->_imu->pos(); - Eigen::Vector3d v_0 = state->_imu->vel(); - - // k1 ================ - Eigen::Vector4d dq_0 = {0, 0, 0, 1}; - Eigen::Vector4d q0_dot = 0.5 * Omega(w_hat) * dq_0; - Eigen::Vector3d p0_dot = v_0; - Eigen::Matrix3d R_Gto0 = quat_2_Rot(quat_multiply(dq_0, q_0)); - Eigen::Vector3d v0_dot = R_Gto0.transpose() * a_hat - _gravity; - - Eigen::Vector4d k1_q = q0_dot * dt; - Eigen::Vector3d k1_p = p0_dot * dt; - Eigen::Vector3d k1_v = v0_dot * dt; - - // k2 ================ - w_hat += 0.5 * w_alpha * dt; - a_hat += 0.5 * a_jerk * dt; - - Eigen::Vector4d dq_1 = quatnorm(dq_0 + 0.5 * k1_q); - // Eigen::Vector3d p_1 = p_0+0.5*k1_p; - Eigen::Vector3d v_1 = v_0 + 0.5 * k1_v; - - Eigen::Vector4d q1_dot = 0.5 * Omega(w_hat) * dq_1; - Eigen::Vector3d p1_dot = v_1; - Eigen::Matrix3d R_Gto1 = quat_2_Rot(quat_multiply(dq_1, q_0)); - Eigen::Vector3d v1_dot = R_Gto1.transpose() * a_hat - _gravity; - - Eigen::Vector4d k2_q = q1_dot * dt; - Eigen::Vector3d k2_p = p1_dot * dt; - Eigen::Vector3d k2_v = v1_dot * dt; - - // k3 ================ - Eigen::Vector4d dq_2 = quatnorm(dq_0 + 0.5 * k2_q); - // Eigen::Vector3d p_2 = p_0+0.5*k2_p; - Eigen::Vector3d v_2 = v_0 + 0.5 * k2_v; - - Eigen::Vector4d q2_dot = 0.5 * Omega(w_hat) * dq_2; - Eigen::Vector3d p2_dot = v_2; - Eigen::Matrix3d R_Gto2 = quat_2_Rot(quat_multiply(dq_2, q_0)); - Eigen::Vector3d v2_dot = R_Gto2.transpose() * a_hat - _gravity; - - Eigen::Vector4d k3_q = q2_dot * dt; - Eigen::Vector3d k3_p = p2_dot * dt; - Eigen::Vector3d k3_v = v2_dot * dt; - - // k4 ================ - w_hat += 0.5 * w_alpha * dt; - a_hat += 0.5 * a_jerk * dt; - - Eigen::Vector4d dq_3 = quatnorm(dq_0 + k3_q); - // Eigen::Vector3d p_3 = p_0+k3_p; - Eigen::Vector3d v_3 = v_0 + k3_v; - - Eigen::Vector4d q3_dot = 0.5 * Omega(w_hat) * dq_3; - Eigen::Vector3d p3_dot = v_3; - Eigen::Matrix3d R_Gto3 = quat_2_Rot(quat_multiply(dq_3, q_0)); - Eigen::Vector3d v3_dot = R_Gto3.transpose() * a_hat - _gravity; - - Eigen::Vector4d k4_q = q3_dot * dt; - Eigen::Vector3d k4_p = p3_dot * dt; - Eigen::Vector3d k4_v = v3_dot * dt; - - // y+dt ================ - Eigen::Vector4d dq = quatnorm(dq_0 + (1.0 / 6.0) * k1_q + (1.0 / 3.0) * k2_q + (1.0 / 3.0) * k3_q + (1.0 / 6.0) * k4_q); - new_q = quat_multiply(dq, q_0); - new_p = p_0 + (1.0 / 6.0) * k1_p + (1.0 / 3.0) * k2_p + (1.0 / 3.0) * k3_p + (1.0 / 6.0) * k4_p; - new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v; -} - -void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, - Eigen::Matrix &Xi_sum) { - - // useful identities - Eigen::Matrix3d I_3x3 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d Z_3x1 = Eigen::MatrixXd::Zero(3, 1); - - // now begin the integration - double w_norm = w_hat.norm(); - double d_th = w_norm * d_t; - Eigen::Vector3d k_hat; - if (w_norm < 1e-8) { - k_hat << Z_3x1; - } else { - k_hat << w_hat / w_norm; - } - - // comupute usefull identities - double d_t2 = std::pow(d_t, 2); - double d_t3 = std::pow(d_t, 3); - ; - double w_norm2 = std::pow(w_norm, 2); - double w_norm3 = std::pow(w_norm, 3); - double cos_dth = cos(d_th); - double sin_dth = sin(d_th); - double d_th2 = std::pow(d_th, 2); - double d_th3 = std::pow(d_th, 3); - Eigen::Matrix3d sK = ov_core::skew_x(k_hat); - Eigen::Matrix3d sK2 = sK * sK; - Eigen::Matrix3d sA = ov_core::skew_x(a_hat); - - // based on the delta theta, let's decide which integration will be used - bool small_w = (w_norm < 1.0 / 180 * M_PI / 2); - bool small_th = (d_th < 1.0 / 180 * M_PI / 2); - - // integration components will be used later - Eigen::Matrix3d R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; - - // now the R and J can be computed - R_k1tok = I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2; - if (!small_th) { - Jr_k1tok = I_3x3 - (1.0 - cos_dth) / d_th * sK + (d_th - sin_dth) / d_th * sK2; - } else { - Jr_k1tok = I_3x3 - sin_dth * sK + (1.0 - cos_dth) * sK2; - } - - // now begin the integration - if (!small_w) { - - // first order rotation integration with constant omega - Xi_1 = I_3x3 * d_t + (1.0 - cos_dth) / w_norm * sK + (d_t - sin_dth / w_norm) * sK2; - - // second order rotation integration with constat omega - Xi_2 = 1.0 / 2 * d_t2 * I_3x3 + (d_th - sin_dth) / w_norm2 * sK + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sK2; - - // first order RAJ integratioin with constant omega and constant acc - Xi_3 = 1.0 / 2 * d_t2 * sA + (sin_dth - d_th) / w_norm2 * sA * sK + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA + - (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sA * sK2 + - (1.0 / 2 * d_t2 - (1.0 - cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - - (3 * sin_dth - 2 * d_th - d_th * cos_dth) / w_norm2 * k_hat.dot(a_hat) * sK2; - - // second order RAJ integration with constant omega and constant acc - Xi_4 = 1.0 / 6 * d_t3 * sA + (2 * (1.0 - cos_dth) - d_th2) / (2 * w_norm3) * sA * sK + - ((2 * (1.0 - cos_dth) - d_th * sin_dth) / w_norm3) * sK * sA + ((sin_dth - d_th) / w_norm3 + d_t3 / 6) * sA * sK2 + - ((d_th - 2 * sin_dth + 1.0 / 6 * d_th3 + d_th * cos_dth) / w_norm3) * (sK2 * sA + k_hat.dot(a_hat) * sK) + - (4 * cos_dth - 4 + d_th2 + d_th * sin_dth) / w_norm3 * k_hat.dot(a_hat) * sK2; - - } else { - - // first order rotation - Xi_1 = d_t * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); - - // second order rotation - Xi_2 = 1.0 / 2 * d_t * Xi_1; - // iint_R = 1.0/2 * d_t2 * (I_3x3 + sin_dth * sK + (1.0-cos_dth) * sK2); - - // first order RAJ - Xi_3 = 1.0 / 2 * d_t2 * - (sA + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); - - // second order RAJ - Xi_4 = 1.0 / 3 * d_t * Xi_3; - } - - // store the integrated parameters - Xi_sum << R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; - - // we are good to go - return; + G.block(th_id, 0, 3, 3) = -Jr * dt * R_wtoI * Dw; + G.block(th_id, 3, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block(v_id, 3, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; + G.block(p_id, 3, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; + G.block(bg_id, 6, 3, 3) = dt * Eigen::Matrix3d::Identity(); + G.block(ba_id, 9, 3, 3) = dt * Eigen::Matrix3d::Identity(); } Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected) { Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); - Eigen::Vector3d e_1 = I_3x3.block<3, 1>(0, 0); - Eigen::Vector3d e_2 = I_3x3.block<3, 1>(0, 1); - Eigen::Vector3d e_3 = I_3x3.block<3, 1>(0, 2); - + Eigen::Vector3d e_1 = I_3x3.block(0, 0, 3, 1); + Eigen::Vector3d e_2 = I_3x3.block(0, 1, 3, 1); + Eigen::Vector3d e_3 = I_3x3.block(0, 2, 3, 1); double w_1 = w_uncorrected(0); double w_2 = w_uncorrected(1); double w_3 = w_uncorrected(2); + assert(state->_options.do_calib_imu_intrinsics); - // intrinsic parameters - Eigen::MatrixXd H_Dw; - if (state->_options.do_calib_imu_intrinsics) { - H_Dw = Eigen::MatrixXd::Zero(3, 6); - if (state->_options.imu_model == 0) { - // Kalibr model - H_Dw << w_1 * I_3x3, w_2 * e_2, w_2 * e_3, w_3 * e_3; - } else { - // RPNG model - H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * I_3x3; - } + Eigen::MatrixXd H_Dw = Eigen::MatrixXd::Zero(3, 6); + if (state->_options.imu_model == 0) { + H_Dw << w_1 * I_3x3, w_2 * e_2, w_2 * e_3, w_3 * e_3; + } else { + H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * I_3x3; } - - // we are good to go return H_Dw; } Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected) { Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); - Eigen::Vector3d e_1 = I_3x3.block<3, 1>(0, 0); - Eigen::Vector3d e_2 = I_3x3.block<3, 1>(0, 1); - Eigen::Vector3d e_3 = I_3x3.block<3, 1>(0, 2); - + Eigen::Vector3d e_1 = I_3x3.block(0, 0, 3, 1); + Eigen::Vector3d e_2 = I_3x3.block(0, 1, 3, 1); + Eigen::Vector3d e_3 = I_3x3.block(0, 2, 3, 1); double a_1 = a_uncorrected(0); double a_2 = a_uncorrected(1); double a_3 = a_uncorrected(2); + assert(state->_options.do_calib_imu_intrinsics); - // intrinsic parameters - Eigen::MatrixXd H_Da; - if (state->_options.do_calib_imu_intrinsics) { - H_Da = Eigen::MatrixXd::Zero(3, 6); - if (state->_options.imu_model == 0) { - // kalibr model - H_Da << a_1 * I_3x3, a_2 * e_2, a_2 * e_3, a_3 * e_3; - } else { - // RPNG model - H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * I_3x3; - } + Eigen::MatrixXd H_Da = Eigen::MatrixXd::Zero(3, 6); + if (state->_options.imu_model == 0) { + H_Da << a_1 * I_3x3, a_2 * e_2, a_2 * e_3, a_3 * e_3; + } else { + H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * I_3x3; } - - // we are good to go return H_Da; } @@ -1010,13 +972,10 @@ Eigen::MatrixXd Propagator::compute_H_Tg(std::shared_ptr state, const Eig double a_1 = a_inI(0); double a_2 = a_inI(1); double a_3 = a_inI(2); + assert(state->_options.do_calib_imu_intrinsics); + assert(state->_options.do_calib_imu_g_sensitivity); - // intrinsic parameters Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3, 9); - if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { - H_Tg << a_1 * I_3x3, a_2 * I_3x3, a_3 * I_3x3; - } - - // we are good to go + H_Tg << a_1 * I_3x3, a_2 * I_3x3, a_3 * I_3x3; return H_Tg; } \ No newline at end of file diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index e9b02d7d5..e5f731928 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -53,7 +53,7 @@ State::State(StateOptions &options) { } else { // upper triangular of the matrix (column-wise) Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); - _imu_default << 1.0, 0.0, 1.0, 0.0, 0.0, 1.0; + _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; _calib_imu_dw->set_fej(_imu_default); _calib_imu_dw->set_fej(_imu_default); _calib_imu_da->set_fej(_imu_default); @@ -135,20 +135,17 @@ State::State(StateOptions &options) { // Finally, set some of our priors for our calibration parameters if (_options.do_calib_imu_intrinsics) { - _Cov.block<6, 6>(_calib_imu_dw->id(), _calib_imu_dw->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); - _Cov.block<6, 6>(_calib_imu_da->id(), _calib_imu_da->id()) = std::pow(0.008, 2) * Eigen::Matrix::Identity(); + _Cov.block(_calib_imu_dw->id(), _calib_imu_dw->id(), 6, 6) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block(_calib_imu_da->id(), _calib_imu_da->id(), 6, 6) = std::pow(0.008, 2) * Eigen::Matrix::Identity(); if (_options.do_calib_imu_g_sensitivity) { - _Cov.block<9, 9>(_calib_imu_tg->id(), _calib_imu_tg->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block(_calib_imu_tg->id(), _calib_imu_tg->id(), 9, 9) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } if (_options.imu_model == 0) { - // if kalibr model, R_GyrotoI is calibrated - _Cov.block<3, 3>(_calib_imu_GYROtoIMU->id(), _calib_imu_GYROtoIMU->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block(_calib_imu_GYROtoIMU->id(), _calib_imu_GYROtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); } else { - // if rpng model, R_AcctoI is calibrated - _Cov.block<3, 3>(_calib_imu_ACCtoIMU->id(), _calib_imu_ACCtoIMU->id()) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); + _Cov.block(_calib_imu_ACCtoIMU->id(), _calib_imu_ACCtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); } } - if (_options.do_calib_camera_timeoffset) { _Cov(_calib_dt_CAMtoIMU->id(), _calib_dt_CAMtoIMU->id()) = std::pow(0.01, 2); } @@ -167,43 +164,3 @@ State::State(StateOptions &options) { } } } - -Eigen::Matrix3d State::Dw() { - Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if (_options.imu_model == 0) { - // if kalibr model, lower triangular of the matrix is used - Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), - _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); - } else { - // if rpng model, upper triangular of the matrix is used - Dw << _calib_imu_dw->value()(0), _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), - _calib_imu_dw->value()(4), 0, 0, _calib_imu_dw->value()(5); - } - return Dw; -} - -Eigen::Matrix3d State::Da() { - Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (_options.imu_model == 0) { - // if kalibr model, lower triangular of the matrix is used - Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), - _calib_imu_da->value()(4), _calib_imu_da->value()(5); - } else { - // if rpng model, upper triangular of the matrix is used - Da << _calib_imu_da->value()(0), _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), - _calib_imu_da->value()(4), 0, 0, _calib_imu_da->value()(5); - } - return Da; -} - -Eigen::Matrix3d State::Tg() { - Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << _calib_imu_tg->value()(0), _calib_imu_tg->value()(3), _calib_imu_tg->value()(6), _calib_imu_tg->value()(1), - _calib_imu_tg->value()(4), _calib_imu_tg->value()(7), _calib_imu_tg->value()(2), _calib_imu_tg->value()(5), _calib_imu_tg->value()(8); - - return Tg; -} - -Eigen::Matrix3d State::R_AcctoI() { return _calib_imu_ACCtoIMU->Rot(); } - -Eigen::Matrix3d State::R_GyrotoI() { return _calib_imu_GYROtoIMU->Rot(); } diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index e3ebf05fe..68e01109a 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -78,23 +78,67 @@ class State { */ int max_covariance_size() { return (int)_Cov.rows(); } + /** + * @brief Gyroscope intrinsic (scale imperfection and axis misalignment) + * @return 3x3 matrix of current imu gyroscope intrinsics + */ + Eigen::Matrix3d Dw() { + Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); + if (_options.imu_model == 0) { + // if kalibr model, lower triangular of the matrix is used + Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), + _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); + } else { + // if rpng model, upper triangular of the matrix is used + Dw << _calib_imu_dw->value()(0), _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), + _calib_imu_dw->value()(4), 0, 0, _calib_imu_dw->value()(5); + } + return Dw; + } - // get Dw - Eigen::Matrix3d Dw(); - // get Da - Eigen::Matrix3d Da(); - // get Tg - Eigen::Matrix3d Tg(); - // get R_AcctoI - Eigen::Matrix3d R_AcctoI(); - // get R_GyrotoI - Eigen::Matrix3d R_GyrotoI(); - - int imu_intrinsic_size() { + /** + * @brief Accelerometer intrinsic (scale imperfection and axis misalignment) + * @return 3x3 matrix of current imu accelerometer intrinsics + */ + Eigen::Matrix3d Da() { + Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); + if (_options.imu_model == 0) { + // if kalibr model, lower triangular of the matrix is used + Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), + _calib_imu_da->value()(4), _calib_imu_da->value()(5); + } else { + // if rpng model, upper triangular of the matrix is used + Da << _calib_imu_da->value()(0), _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), + _calib_imu_da->value()(4), 0, 0, _calib_imu_da->value()(5); + } + return Da; + } + + /** + * @brief Gyroscope gravity sensitivity + * @return 3x3 matrix of current gravity sensitivity + */ + Eigen::Matrix3d Tg() { + Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); + Tg << _calib_imu_tg->value()(0), _calib_imu_tg->value()(3), _calib_imu_tg->value()(6), _calib_imu_tg->value()(1), + _calib_imu_tg->value()(4), _calib_imu_tg->value()(7), _calib_imu_tg->value()(2), _calib_imu_tg->value()(5), + _calib_imu_tg->value()(8); + return Tg; + } + + /** + * @brief Calculates the error state size for imu intrinsics. + * + * This is used to construct our state transition which depends on if we are estimating calibration. + * 15 if doing intrinsics, another +9 if doing grav sensitivity + * + * @return size of error state + */ + int imu_intrinsic_size() const { int sz = 0; - if(_options.do_calib_imu_intrinsics){ + if (_options.do_calib_imu_intrinsics) { sz += 15; - if(_options.do_calib_imu_g_sensitivity){ + if (_options.do_calib_imu_g_sensitivity) { sz += 9; } } diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 920b8b90a..63f2803f6 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -162,11 +162,6 @@ struct StateOptions { PRINT_DEBUG(" - feat_rep_msckf: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); PRINT_DEBUG(" - feat_rep_slam: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_slam).c_str()); PRINT_DEBUG(" - feat_rep_aruco: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); - if ((do_calib_imu_intrinsics || do_calib_imu_g_sensitivity) && !use_analytic_integration) { - PRINT_ERROR(RED "calibrated IMU model selected, but not using analytical integration!\n" RESET); - PRINT_ERROR(RED "please enable analytical integration to perform this calibration!\n" RESET); - std::exit(EXIT_FAILURE); - } } }; From 765db5d10e608283da6bd824d14ce33bd83cd7b3 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 16:59:39 -0500 Subject: [PATCH 22/60] more cleanup, add enum --- ov_core/src/utils/sensor_data.h | 27 ++++++++++++++-------- ov_msckf/src/core/VioManager.cpp | 8 +++---- ov_msckf/src/core/VioManagerOptions.h | 8 +++---- ov_msckf/src/ros/ROS2Visualizer.cpp | 9 +++++--- ov_msckf/src/ros/RosVisualizerHelper.h | 31 +++++++++++++------------- ov_msckf/src/sim/Simulator.cpp | 4 ++-- ov_msckf/src/state/Propagator.cpp | 10 ++++----- ov_msckf/src/state/Propagator.h | 8 ++++--- ov_msckf/src/state/State.cpp | 6 ++--- ov_msckf/src/state/State.h | 4 ++-- ov_msckf/src/state/StateHelper.h | 1 - ov_msckf/src/state/StateOptions.h | 9 ++++---- 12 files changed, 69 insertions(+), 56 deletions(-) diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index ad760ee78..d0a6bf7b9 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -81,27 +81,36 @@ struct CameraData { }; /** - * @brief Struct of our imu noise parameters + * @brief Struct of our imu noise and intrinsic parameters. + * + * For details on each specific model see @ref imu_intrinsics */ struct ImuConfig { - /// imu model, 0: Kalibr model and 1: RPNG model - // TODO: convert this to an enum!!! - int imu_model = 0; + /** + * @brief What model our IMU is + */ + enum ImuModel { + KALIBR, + RPNG + }; + + /// IMU model our intrinsic calibration is using. + ImuModel imu_model = ImuModel::KALIBR; - /// the columnwise elements for Dw + /// Gyroscope IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse) Eigen::Matrix vec_dw; - /// the columnwise elements for Da + /// Accelerometer IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse) Eigen::Matrix vec_da; - /// the ccolumnwise elements for Tg + /// Gyroscope gravity sensitivity (scale imperfection and axis misalignment, column-wise) Eigen::Matrix vec_tg; - /// the JPL quat for R_ACCtoIMU + /// Rotation from gyroscope frame to the "IMU" accelerometer frame Eigen::Matrix q_ACCtoIMU; - /// the JPL quat for R_GYROtoIMU + /// Rotation from accelerometer to the "IMU" gyroscope frame frame Eigen::Matrix q_GYROtoIMU; /// Gyroscope white noise (rad/s/sqrt(hz)) diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 56334dd81..086293078 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -724,7 +724,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) } // Debug for imu intrinsics - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { PRINT_INFO("Dw = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); @@ -732,7 +732,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); } - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::RPNG) { PRINT_INFO("Dw = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); @@ -746,11 +746,11 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), state->_calib_imu_tg->value()(6), state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); } - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { PRINT_INFO("q_GYROtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_GYROtoIMU->value()(0), state->_calib_imu_GYROtoIMU->value()(1), state->_calib_imu_GYROtoIMU->value()(2), state->_calib_imu_GYROtoIMU->value()(3)); } - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::RPNG) { PRINT_INFO("q_ACCtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_ACCtoIMU->value()(0), state->_calib_imu_ACCtoIMU->value()(1), state->_calib_imu_ACCtoIMU->value()(2), state->_calib_imu_ACCtoIMU->value()(3)); } diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index c430dcb81..a4837d18e 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -283,9 +283,9 @@ struct VioManagerOptions { std::string imu_model_str = "kalibr"; parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); // might be redundant if (imu_model_str == "kalibr" || imu_model_str == "calibrated") { - imu_config.imu_model = 0; + imu_config.imu_model = ov_core::ImuConfig::ImuModel::KALIBR; } else if (imu_model_str == "rpng") { - imu_config.imu_model = 1; + imu_config.imu_model = ov_core::ImuConfig::ImuModel::RPNG; } else { PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); std::exit(EXIT_FAILURE); @@ -312,7 +312,7 @@ struct VioManagerOptions { // kalibr model: lower triangular of the matrix and R_GYROtoI // rpng model: upper triangular of the matrix and R_ACCtoI - if (imu_config.imu_model == 0) { + if (imu_config.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { imu_config.vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); imu_config.vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); } else { @@ -355,7 +355,7 @@ struct VioManagerOptions { } PRINT_DEBUG("IMU PARAMETERS:\n"); std::stringstream ss; - ss << "imu model:" << ((imu_config.imu_model == 0) ? "kalibr" : "rpng") << std::endl; + ss << "imu model:" << ((imu_config.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) ? "kalibr" : "rpng") << std::endl; ss << "Dw (columnwise):" << imu_config.vec_dw.transpose() << std::endl; ss << "Da (columnwise):" << imu_config.vec_da.transpose() << std::endl; ss << "Tg (columnwise):" << imu_config.vec_tg.transpose() << std::endl; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 873447d47..51a5f91d2 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -109,15 +109,18 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p // Open the files of_state_est.open(filepath_est.c_str()); of_state_std.open(filepath_std.c_str()); - of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; - of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; + of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" + << std::endl; + of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" + << std::endl; // Groundtruth if we are simulating if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); of_state_gt.open(filepath_gt.c_str()); - of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; + of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" + << std::endl; } } } diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 49fda921b..14aac043b 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -297,13 +297,11 @@ class RosVisualizerHelper { << sim->get_true_parameters().imu_config.vec_tg(4) << " " << sim->get_true_parameters().imu_config.vec_tg(5) << " " << sim->get_true_parameters().imu_config.vec_tg(6) << " " << sim->get_true_parameters().imu_config.vec_tg(7) << " " << sim->get_true_parameters().imu_config.vec_tg(8) << " "; - of_state_gt << sim->get_true_parameters().imu_config.q_GYROtoIMU(0) << " " - << sim->get_true_parameters().imu_config.q_GYROtoIMU(1) << " " - << sim->get_true_parameters().imu_config.q_GYROtoIMU(2) << " " + of_state_gt << sim->get_true_parameters().imu_config.q_GYROtoIMU(0) << " " << sim->get_true_parameters().imu_config.q_GYROtoIMU(1) + << " " << sim->get_true_parameters().imu_config.q_GYROtoIMU(2) << " " << sim->get_true_parameters().imu_config.q_GYROtoIMU(3) << " "; - of_state_gt << sim->get_true_parameters().imu_config.q_ACCtoIMU(0) << " " - << sim->get_true_parameters().imu_config.q_ACCtoIMU(1) << " " - << sim->get_true_parameters().imu_config.q_ACCtoIMU(2) << " " + of_state_gt << sim->get_true_parameters().imu_config.q_ACCtoIMU(0) << " " << sim->get_true_parameters().imu_config.q_ACCtoIMU(1) + << " " << sim->get_true_parameters().imu_config.q_ACCtoIMU(2) << " " << sim->get_true_parameters().imu_config.q_ACCtoIMU(3) << " "; // New line of_state_gt << endl; @@ -406,8 +404,9 @@ class RosVisualizerHelper { of_state_std.precision(8); // imu intrinsics: dw - of_state_est << state->_calib_imu_dw->value()(0) << " " << state->_calib_imu_dw->value()(1) << " " << state->_calib_imu_dw->value()(2) << " " - << state->_calib_imu_dw->value()(3) << " " << state->_calib_imu_dw->value()(4) << " " << state->_calib_imu_dw->value()(5) << " "; + of_state_est << state->_calib_imu_dw->value()(0) << " " << state->_calib_imu_dw->value()(1) << " " << state->_calib_imu_dw->value()(2) + << " " << state->_calib_imu_dw->value()(3) << " " << state->_calib_imu_dw->value()(4) << " " + << state->_calib_imu_dw->value()(5) << " "; if (state->_options.do_calib_imu_intrinsics) { int index_dw = state->_calib_imu_dw->id(); of_state_std << std::sqrt(cov(index_dw + 0, index_dw + 0)) << " " << std::sqrt(cov(index_dw + 1, index_dw + 1)) << " " @@ -419,8 +418,9 @@ class RosVisualizerHelper { } // imu intrinsics: da - of_state_est << state->_calib_imu_da->value()(0) << " " << state->_calib_imu_da->value()(1) << " " << state->_calib_imu_da->value()(2) << " " - << state->_calib_imu_da->value()(3) << " " << state->_calib_imu_da->value()(4) << " " << state->_calib_imu_da->value()(5) << " "; + of_state_est << state->_calib_imu_da->value()(0) << " " << state->_calib_imu_da->value()(1) << " " << state->_calib_imu_da->value()(2) + << " " << state->_calib_imu_da->value()(3) << " " << state->_calib_imu_da->value()(4) << " " + << state->_calib_imu_da->value()(5) << " "; if (state->_options.do_calib_imu_intrinsics) { int index_da = state->_calib_imu_da->id(); of_state_std << std::sqrt(cov(index_da + 0, index_da + 0)) << " " << std::sqrt(cov(index_da + 1, index_da + 1)) << " " @@ -432,9 +432,10 @@ class RosVisualizerHelper { } // imu intrinsics: tg - of_state_est << state->_calib_imu_tg->value()(0) << " " << state->_calib_imu_tg->value()(1) << " " << state->_calib_imu_tg->value()(2) << " " - << state->_calib_imu_tg->value()(3) << " " << state->_calib_imu_tg->value()(4) << " " << state->_calib_imu_tg->value()(5) << " " - << state->_calib_imu_tg->value()(6) << " " << state->_calib_imu_tg->value()(7) << " " << state->_calib_imu_tg->value()(8) << " "; + of_state_est << state->_calib_imu_tg->value()(0) << " " << state->_calib_imu_tg->value()(1) << " " << state->_calib_imu_tg->value()(2) + << " " << state->_calib_imu_tg->value()(3) << " " << state->_calib_imu_tg->value()(4) << " " + << state->_calib_imu_tg->value()(5) << " " << state->_calib_imu_tg->value()(6) << " " << state->_calib_imu_tg->value()(7) + << " " << state->_calib_imu_tg->value()(8) << " "; if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { int index_tg = state->_calib_imu_tg->id(); of_state_std << std::sqrt(cov(index_tg + 0, index_tg + 0)) << " " << std::sqrt(cov(index_tg + 1, index_tg + 1)) << " " @@ -451,7 +452,7 @@ class RosVisualizerHelper { // imu intrinsics: kalibr R_gyrotoI of_state_est << state->_calib_imu_GYROtoIMU->value()(0) << " " << state->_calib_imu_GYROtoIMU->value()(1) << " " << state->_calib_imu_GYROtoIMU->value()(2) << " " << state->_calib_imu_GYROtoIMU->value()(3) << " "; - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 0) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { int index_wtoI = state->_calib_imu_GYROtoIMU->id(); of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " " << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " "; @@ -462,7 +463,7 @@ class RosVisualizerHelper { // imu intrinsics: rpng R_acctoI of_state_est << state->_calib_imu_ACCtoIMU->value()(0) << " " << state->_calib_imu_ACCtoIMU->value()(1) << " " << state->_calib_imu_ACCtoIMU->value()(2) << " " << state->_calib_imu_ACCtoIMU->value()(3) << " "; - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == 1) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ov_core::ImuConfig::ImuModel::RPNG) { int index_atoI = state->_calib_imu_ACCtoIMU->id(); of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " " << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " "; diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 0f8930f5b..9b83542a9 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -233,7 +233,7 @@ void Simulator::perturb_parameters(std::mt19937 gen_state, VioManagerOptions &pa params_.imu_config.vec_dw(j) += 0.004 * w(gen_state); params_.imu_config.vec_da(j) += 0.004 * w(gen_state); } - if (params_.state_options.imu_model == 0) { + if (params_.state_options.imu_model == ImuConfig::ImuModel::KALIBR) { Eigen::Vector3d w_vec; w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); params_.imu_config.q_GYROtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.q_GYROtoIMU)); @@ -335,7 +335,7 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto // - rpng: upper triangular of the matrix is used Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (params.imu_config.imu_model == 0) { + if (params.imu_config.imu_model == ImuConfig::ImuModel::KALIBR) { Dw << params.imu_config.vec_dw(0), 0, 0, params.imu_config.vec_dw(1), params.imu_config.vec_dw(3), 0, params.imu_config.vec_dw(2), params.imu_config.vec_dw(4), params.imu_config.vec_dw(5); Da << params.imu_config.vec_da(0), 0, 0, params.imu_config.vec_da(1), params.imu_config.vec_da(3), 0, params.imu_config.vec_da(2), diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 1aab9578c..346d1ded9 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -111,7 +111,7 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest if (state->_options.do_calib_imu_g_sensitivity) { Phi_order.push_back(state->_calib_imu_tg); } - if (state->_options.imu_model == 0) { + if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { Phi_order.push_back(state->_calib_imu_GYROtoIMU); } else { Phi_order.push_back(state->_calib_imu_ACCtoIMU); @@ -683,7 +683,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Tg_id = local_size; local_size += state->_calib_imu_tg->size(); } - if (state->_options.imu_model == 0) { + if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); } else { @@ -828,7 +828,7 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d Tg_id = local_size; local_size += state->_calib_imu_tg->size(); } - if (state->_options.imu_model == 0) { + if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); } else { @@ -938,7 +938,7 @@ Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eig assert(state->_options.do_calib_imu_intrinsics); Eigen::MatrixXd H_Dw = Eigen::MatrixXd::Zero(3, 6); - if (state->_options.imu_model == 0) { + if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { H_Dw << w_1 * I_3x3, w_2 * e_2, w_2 * e_3, w_3 * e_3; } else { H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * I_3x3; @@ -958,7 +958,7 @@ Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eig assert(state->_options.do_calib_imu_intrinsics); Eigen::MatrixXd H_Da = Eigen::MatrixXd::Zero(3, 6); - if (state->_options.imu_model == 0) { + if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { H_Da << a_1 * I_3x3, a_2 * e_2, a_2 * e_3, a_3 * e_3; } else { H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * I_3x3; diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 2c60c084f..b29dadb9c 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -369,6 +369,7 @@ class Propagator { * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 * \end{bmatrix} * \f} + * * For rpng model: * \f{align*}{ * \mathbf{H}_{Dw} & = @@ -376,6 +377,7 @@ class Propagator { * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 * \end{bmatrix} * \f} + * * @param state Pointer to state * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed */ @@ -385,7 +387,6 @@ class Propagator { * @brief compute the Jacobians for Da * * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * * For kalibr model * \f{align*}{ * \mathbf{H}_{Da} & = @@ -393,6 +394,7 @@ class Propagator { * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 * \end{bmatrix} * \f} + * * For rpng: * \f{align*}{ * \mathbf{H}_{Da} & = @@ -400,6 +402,7 @@ class Propagator { * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 * \end{bmatrix} * \f} + * * @param state Pointer to state * @param a_uncorrected Linear acceleration in gyro frame with bias removed */ @@ -409,14 +412,13 @@ class Propagator { * @brief compute the Jacobians for Tg * * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * - * * \f{align*}{ * \mathbf{H}_{Tg} & = * \begin{bmatrix} * {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 * \end{bmatrix} * \f} + * * @param state Pointer to state * @param a_inI Linear acceleration with bias removed */ diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index e5f731928..48c0c6295 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -42,7 +42,7 @@ State::State(StateOptions &options) { // NOTE: since if calibrating these will evolve / be correlated during propagation _calib_imu_dw = std::make_shared(6); _calib_imu_da = std::make_shared(6); - if (options.imu_model == 0) { + if (options.imu_model == ImuConfig::ImuModel::KALIBR) { // lower triangular of the matrix (column-wise) Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; @@ -83,7 +83,7 @@ State::State(StateOptions &options) { // If kalibr model, R_GYROtoIMU is calibrated // If rpng model, R_ACCtoIMU is calibrated - if (options.imu_model == 0) { + if (options.imu_model == ImuConfig::ImuModel::KALIBR) { _calib_imu_GYROtoIMU->set_local_id(current_id); _variables.push_back(_calib_imu_GYROtoIMU); current_id += _calib_imu_GYROtoIMU->size(); @@ -140,7 +140,7 @@ State::State(StateOptions &options) { if (_options.do_calib_imu_g_sensitivity) { _Cov.block(_calib_imu_tg->id(), _calib_imu_tg->id(), 9, 9) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } - if (_options.imu_model == 0) { + if (_options.imu_model == ImuConfig::ImuModel::KALIBR) { _Cov.block(_calib_imu_GYROtoIMU->id(), _calib_imu_GYROtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); } else { _Cov.block(_calib_imu_ACCtoIMU->id(), _calib_imu_ACCtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 68e01109a..e0bd79989 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -84,7 +84,7 @@ class State { */ Eigen::Matrix3d Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if (_options.imu_model == 0) { + if (_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { // if kalibr model, lower triangular of the matrix is used Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); @@ -102,7 +102,7 @@ class State { */ Eigen::Matrix3d Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (_options.imu_model == 0) { + if (_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { // if kalibr model, lower triangular of the matrix is used Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), _calib_imu_da->value()(4), _calib_imu_da->value()(5); diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index 3f32a81af..ebebecd33 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -255,7 +255,6 @@ class StateHelper { StateHelper() {} }; - } // namespace ov_msckf #endif // OV_MSCKF_STATE_HELPER_H diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 63f2803f6..8563b0705 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -25,8 +25,7 @@ #include "types/LandmarkRepresentation.h" #include "utils/opencv_yaml_parse.h" #include "utils/print.h" - -#include +#include "utils/sensor_data.h" namespace ov_msckf { @@ -63,7 +62,7 @@ struct StateOptions { bool do_calib_imu_g_sensitivity = false; /// Indicator to use which model, 0: kalibr and 1: rpng - int imu_model = 0; + ov_core::ImuConfig::ImuModel imu_model = ov_core::ImuConfig::ImuModel::KALIBR; /// Max clone size of sliding window int max_clone_size = 11; @@ -130,9 +129,9 @@ struct StateOptions { std::string imu_model_str = "kalibr"; parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); if (imu_model_str == "kalibr" || imu_model_str == "calibrated") { - imu_model = 0; + imu_model = ov_core::ImuConfig::ImuModel::KALIBR; } else if (imu_model_str == "rpng") { - imu_model = 1; + imu_model = ov_core::ImuConfig::ImuModel::RPNG; } else { PRINT_ERROR(RED "invalid imu model: %s\n" RESET, imu_model_str.c_str()); std::exit(EXIT_FAILURE); From 0eaac5325c13a2f211239cc00f9528ee18b26081 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 17:06:28 -0500 Subject: [PATCH 23/60] small changes --- ov_msckf/src/state/Propagator.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 346d1ded9..be03a4e8c 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -599,7 +599,7 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const // second order rotation integration with constat omega Xi_2 = 1.0 / 2 * d_t2 * I_3x3 + (d_th - sin_dth) / w_norm2 * sK + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sK2; - // first order RAJ integratioin with constant omega and constant acc + // first order RAJ integration with constant omega and constant acc Xi_3 = 1.0 / 2 * d_t2 * sA + (sin_dth - d_th) / w_norm2 * sA * sK + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sA * sK2 + (1.0 / 2 * d_t2 - (1.0 - cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - @@ -712,11 +712,11 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains gravity correction already - Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3); - Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6); - Eigen::Matrix3d Jr = Xi_sum.block<3, 3>(0, 9); - Eigen::Matrix3d Xi_3 = Xi_sum.block<3, 3>(0, 12); - Eigen::Matrix3d Xi_4 = Xi_sum.block<3, 3>(0, 15); + Eigen::Matrix3d Xi_1 = Xi_sum.block(0, 3, 3, 3); + Eigen::Matrix3d Xi_2 = Xi_sum.block(0, 6, 3, 3); + Eigen::Matrix3d Jr = Xi_sum.block(0, 9, 3, 3); + Eigen::Matrix3d Xi_3 = Xi_sum.block(0, 12, 3, 3); + Eigen::Matrix3d Xi_4 = Xi_sum.block(0, 15, 3, 3); // for th F.block(th_id, th_id, 3, 3) = dR_ktok1; @@ -724,17 +724,17 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d F.block(v_id, th_id, 3, 3) = -skew_x(new_v - v_k + _gravity * dt) * R_k.transpose(); // for p - F.block<3, 3>(p_id, p_id, 3, 3).setIdentity(); + F.block(p_id, p_id, 3, 3).setIdentity(); // for v - F.block<3, 3>(p_id, v_id, 3, 3) = Eigen::Matrix3d::Identity() * dt; - F.block<3, 3>(v_id, v_id, 3, 3).setIdentity(); + F.block(p_id, v_id, 3, 3) = Eigen::Matrix3d::Identity() * dt; + F.block(v_id, v_id, 3, 3).setIdentity(); // for bg - F.block<3, 3>(th_id, bg_id, 3, 3) = -Jr * dt * R_wtoI * Dw; - F.block<3, 3>(p_id, bg_id, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI * Dw; - F.block<3, 3>(v_id, bg_id, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI * Dw; - F.block<3, 3>(bg_id, bg_id, 3, 3).setIdentity(); + F.block(th_id, bg_id, 3, 3) = -Jr * dt * R_wtoI * Dw; + F.block(p_id, bg_id, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI * Dw; + F.block(v_id, bg_id, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI * Dw; + F.block(bg_id, bg_id, 3, 3).setIdentity(); // for ba F.block(th_id, ba_id, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; From 44a2bfda8556ada694ef05f7bc25afec20866f58 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 6 Jan 2022 18:02:03 -0500 Subject: [PATCH 24/60] fix error in X3, more formating, mono is inconsistent still... --- config/rpng_sim/estimator_config.yaml | 2 +- ov_msckf/launch/simulation.launch | 2 +- ov_msckf/src/core/VioManager.cpp | 5 ++++ ov_msckf/src/core/VioManagerOptions.h | 8 +++---- ov_msckf/src/state/Propagator.cpp | 17 +++++++------- ov_msckf/src/state/Propagator.h | 2 +- ov_msckf/src/state/State.cpp | 34 +++++++++++++-------------- 7 files changed, 37 insertions(+), 33 deletions(-) diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index ea1e53e8b..48d7b3484 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -107,7 +107,7 @@ sim_seed_preturb: 0 sim_seed_measurements: 0 sim_do_perturbation: false sim_traj_path: "src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" -sim_distance_threshold: 1.2 +sim_distance_threshold: 1.1 sim_freq_cam: 10 sim_freq_imu: 400 sim_min_feature_gen_dist: 5.0 diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index b4cb53663..c18db0869 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -33,7 +33,7 @@ - + diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 086293078..211171f03 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -49,10 +49,15 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // Set the IMU intrinsics state->_calib_imu_dw->set_value(params.imu_config.vec_dw); + state->_calib_imu_dw->set_fej(params.imu_config.vec_dw); state->_calib_imu_da->set_value(params.imu_config.vec_da); + state->_calib_imu_da->set_fej(params.imu_config.vec_da); state->_calib_imu_tg->set_value(params.imu_config.vec_tg); + state->_calib_imu_tg->set_fej(params.imu_config.vec_tg); state->_calib_imu_GYROtoIMU->set_value(params.imu_config.q_GYROtoIMU); + state->_calib_imu_GYROtoIMU->set_fej(params.imu_config.q_GYROtoIMU); state->_calib_imu_ACCtoIMU->set_value(params.imu_config.q_ACCtoIMU); + state->_calib_imu_ACCtoIMU->set_fej(params.imu_config.q_ACCtoIMU); // Timeoffset from camera to IMU Eigen::VectorXd temp_camimu_dt; diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index a4837d18e..410909665 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -307,8 +307,8 @@ struct VioManagerOptions { // TODO: error here if this returns a NaN value (i.e. invalid matrix specified) Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); - Eigen::Matrix3d R_AcctoI = R_ItoAcc.transpose(); - Eigen::Matrix3d R_GyrotoI = R_ItoGyro.transpose(); + Eigen::Matrix3d R_ACCtoIMU = R_ItoAcc.transpose(); + Eigen::Matrix3d R_GYROtoIMU = R_ItoGyro.transpose(); // kalibr model: lower triangular of the matrix and R_GYROtoI // rpng model: upper triangular of the matrix and R_ACCtoI @@ -320,8 +320,8 @@ struct VioManagerOptions { imu_config.vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); } imu_config.vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2); - imu_config.q_GYROtoIMU = ov_core::rot_2_quat(R_GyrotoI); - imu_config.q_ACCtoIMU = ov_core::rot_2_quat(R_AcctoI); + imu_config.q_GYROtoIMU = ov_core::rot_2_quat(R_GYROtoIMU); + imu_config.q_ACCtoIMU = ov_core::rot_2_quat(R_ACCtoIMU); } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index be03a4e8c..3a21d4bf1 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -596,16 +596,16 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const // first order rotation integration with constant omega Xi_1 = I_3x3 * d_t + (1.0 - cos_dth) / w_norm * sK + (d_t - sin_dth / w_norm) * sK2; - // second order rotation integration with constat omega + // second order rotation integration with constant omega Xi_2 = 1.0 / 2 * d_t2 * I_3x3 + (d_th - sin_dth) / w_norm2 * sK + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sK2; - // first order RAJ integration with constant omega and constant acc + // first order integration with constant omega and constant acc Xi_3 = 1.0 / 2 * d_t2 * sA + (sin_dth - d_th) / w_norm2 * sA * sK + (sin_dth - d_th * cos_dth) / w_norm2 * sK * sA + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sA * sK2 + - (1.0 / 2 * d_t2 - (1.0 - cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - + (1.0 / 2 * d_t2 + (1.0 - cos_dth - d_th * sin_dth) / w_norm2) * (sK2 * sA + k_hat.dot(a_hat) * sK) - (3 * sin_dth - 2 * d_th - d_th * cos_dth) / w_norm2 * k_hat.dot(a_hat) * sK2; - // second order RAJ integration with constant omega and constant acc + // second order integration with constant omega and constant acc Xi_4 = 1.0 / 6 * d_t3 * sA + (2 * (1.0 - cos_dth) - d_th2) / (2 * w_norm3) * sA * sK + ((2 * (1.0 - cos_dth) - d_th * sin_dth) / w_norm3) * sK * sA + ((sin_dth - d_th) / w_norm3 + d_t3 / 6) * sA * sK2 + ((d_th - 2 * sin_dth + 1.0 / 6 * d_th3 + d_th * cos_dth) / w_norm3) * (sK2 * sA + k_hat.dot(a_hat) * sK) + @@ -613,18 +613,17 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const } else { - // first order rotation + // first order rotation integration with constant omega Xi_1 = d_t * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); - // second order rotation + // second order rotation integration with constant omega Xi_2 = 1.0 / 2 * d_t * Xi_1; - // iint_R = 1.0/2 * d_t2 * (I_3x3 + sin_dth * sK + (1.0-cos_dth) * sK2); - // first order RAJ + // first order integration with constant omega and constant acc Xi_3 = 1.0 / 2 * d_t2 * (sA + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); - // second order RAJ + // second order integration with constant omega and constant acc Xi_4 = 1.0 / 3 * d_t * Xi_3; } diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index b29dadb9c..ef513b709 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -41,7 +41,7 @@ class Propagator { public: /** * @brief Default constructor - * @param noises imu noise characteristics (continuous time) + * @param imu_config imu noise characteristics (continuous time) * @param gravity_mag Global gravity magnitude of the system (normally 9.81) */ Propagator(ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index 48c0c6295..afffed921 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -42,23 +42,23 @@ State::State(StateOptions &options) { // NOTE: since if calibrating these will evolve / be correlated during propagation _calib_imu_dw = std::make_shared(6); _calib_imu_da = std::make_shared(6); - if (options.imu_model == ImuConfig::ImuModel::KALIBR) { - // lower triangular of the matrix (column-wise) - Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); - _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; - _calib_imu_dw->set_value(_imu_default); - _calib_imu_dw->set_fej(_imu_default); - _calib_imu_da->set_value(_imu_default); - _calib_imu_da->set_fej(_imu_default); - } else { - // upper triangular of the matrix (column-wise) - Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); - _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; - _calib_imu_dw->set_fej(_imu_default); - _calib_imu_dw->set_fej(_imu_default); - _calib_imu_da->set_fej(_imu_default); - _calib_imu_da->set_fej(_imu_default); - } + // if (options.imu_model == ImuConfig::ImuModel::KALIBR) { + // // lower triangular of the matrix (column-wise) + // Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); + // _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; + // _calib_imu_dw->set_value(_imu_default); + // _calib_imu_dw->set_fej(_imu_default); + // _calib_imu_da->set_value(_imu_default); + // _calib_imu_da->set_fej(_imu_default); + // } else { + // // upper triangular of the matrix (column-wise) + // Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); + // _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; + // _calib_imu_dw->set_value(_imu_default); + // _calib_imu_dw->set_fej(_imu_default); + // _calib_imu_da->set_value(_imu_default); + // _calib_imu_da->set_fej(_imu_default); + // } _calib_imu_tg = std::make_shared(9); _calib_imu_GYROtoIMU = std::make_shared(); _calib_imu_ACCtoIMU = std::make_shared(); From 56231739695074d57a230c9ff2660197b5b33391 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 7 Jan 2022 09:47:37 -0500 Subject: [PATCH 25/60] fix mask relative paths --- config/euroc_mav/estimator_config.yaml | 1 + config/rpng_ironsides/estimator_config.yaml | 4 ++-- config/tum_vi/estimator_config.yaml | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index e6196b9d4..12d7a1329 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -5,6 +5,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it use_rk4_int: true # if rk4 integration should be used (overrides imu averaging) +use_analytic_int: true # if analytic integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml index 418fb71f7..10fc64c1b 100644 --- a/config/rpng_ironsides/estimator_config.yaml +++ b/config/rpng_ironsides/estimator_config.yaml @@ -96,8 +96,8 @@ up_aruco_chi2_multipler: 1 # masks for our images use_mask: true -mask0: "../../../ov_data/masks/ironsides0.png" #relative to current file -mask1: "../../../ov_data/masks/ironsides1.png" #relative to current file +mask0: "../../ov_data/masks/ironsides0.png" #relative to current file +mask1: "../../ov_data/masks/ironsides1.png" #relative to current file # imu and camera spacial-temporal # imu config should also have the correct noise values diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml index 74c2a953e..19d169589 100644 --- a/config/tum_vi/estimator_config.yaml +++ b/config/tum_vi/estimator_config.yaml @@ -91,8 +91,8 @@ up_aruco_chi2_multipler: 1 # masks for our images use_mask: true -mask0: "../../../ov_data/masks/tumvi0.png" #relative to current file -mask1: "../../../ov_data/masks/tumvi1.png" #relative to current file +mask0: "../../ov_data/masks/tumvi0.png" #relative to current file +mask1: "../../ov_data/masks/tumvi1.png" #relative to current file # imu and camera spacial-temporal # imu config should also have the correct noise values From a09695e148f742f898b1e4a42a43f80e39e2979e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 7 Jan 2022 09:58:25 -0500 Subject: [PATCH 26/60] add mask matrix size check, small cleanups --- ov_msckf/src/core/VioManager.cpp | 10 +++++----- ov_msckf/src/core/VioManagerOptions.h | 23 +++++++++++++++-------- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 211171f03..cd1e75b4a 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -730,23 +730,23 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // Debug for imu intrinsics if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { - PRINT_INFO("Dw = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), + PRINT_INFO("Dw = | %.4f,%.4f,%.4f | %.4f,%.4f | %.4f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); - PRINT_INFO("Da = | %.3f,%.3f,%.3f | %.3f,%.3f | %.3f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), + PRINT_INFO("Da = | %.4f,%.4f,%.4f | %.4f,%.4f | %.4f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); } if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::RPNG) { - PRINT_INFO("Dw = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), + PRINT_INFO("Dw = | %.4f | %.4f,%.4f | %.4f,%.4f,%.4f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); - PRINT_INFO("Da = | %.3f | %.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), + PRINT_INFO("Da = | %.4f | %.4f,%.4f | %.4f,%.4f,%.4f |\n", state->_calib_imu_da->value()(0), state->_calib_imu_da->value()(1), state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); } if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) { - PRINT_INFO("Tg = | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f | %.3f,%.3f,%.3f |\n", state->_calib_imu_tg->value()(0), + PRINT_INFO("Tg = | %.4f,%.4f,%.4f | %.4f,%.4f,%.4f | %.4f,%.4f,%.4f |\n", state->_calib_imu_tg->value()(0), state->_calib_imu_tg->value()(1), state->_calib_imu_tg->value()(2), state->_calib_imu_tg->value()(3), state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), state->_calib_imu_tg->value()(6), state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 410909665..e4206fe6d 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -242,7 +242,6 @@ struct VioManagerOptions { parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "resolution", matrix_wh); matrix_wh.at(0) /= (downsample_cameras) ? 2.0 : 1.0; matrix_wh.at(1) /= (downsample_cameras) ? 2.0 : 1.0; - std::pair wh(matrix_wh.at(0), matrix_wh.at(1)); // Extrinsics Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); @@ -275,7 +274,15 @@ struct VioManagerOptions { PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); std::exit(EXIT_FAILURE); } - masks.insert({i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)}); + cv::Mat mask = cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE); + masks.insert({i, mask}); + if (mask.cols != camera_intrinsics.at(i)->w() || mask.rows != camera_intrinsics.at(i)->h()) { + PRINT_ERROR(RED "VioManager(): mask size does not match camera!\n" RESET); + PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); + PRINT_ERROR(RED "\t- mask%d - %d x %d\n" RESET, mask.cols, mask.rows); + PRINT_ERROR(RED "\t- cam%d - %d x %d\n" RESET, camera_intrinsics.at(i)->w(), camera_intrinsics.at(i)->h()); + std::exit(EXIT_FAILURE); + } } } @@ -296,10 +303,10 @@ struct VioManagerOptions { parser->parse_external("relative_config_imu", "imu0", "Tw", Tw); Eigen::Matrix3d Ta = Eigen::Matrix3d::Identity(); parser->parse_external("relative_config_imu", "imu0", "Ta", Ta); - Eigen::Matrix3d R_ItoAcc = Eigen::Matrix3d::Identity(); - parser->parse_external("relative_config_imu", "imu0", "R_ItoAcc", R_ItoAcc); - Eigen::Matrix3d R_ItoGyro = Eigen::Matrix3d::Identity(); - parser->parse_external("relative_config_imu", "imu0", "R_ItoGyro", R_ItoGyro); + Eigen::Matrix3d R_IMUtoACC = Eigen::Matrix3d::Identity(); + parser->parse_external("relative_config_imu", "imu0", "R_ItoAcc", R_IMUtoACC); + Eigen::Matrix3d R_IMUtoGYRO = Eigen::Matrix3d::Identity(); + parser->parse_external("relative_config_imu", "imu0", "R_ItoGyro", R_IMUtoGYRO); Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); parser->parse_external("relative_config_imu", "imu0", "Tg", Tg); @@ -307,8 +314,8 @@ struct VioManagerOptions { // TODO: error here if this returns a NaN value (i.e. invalid matrix specified) Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); - Eigen::Matrix3d R_ACCtoIMU = R_ItoAcc.transpose(); - Eigen::Matrix3d R_GYROtoIMU = R_ItoGyro.transpose(); + Eigen::Matrix3d R_ACCtoIMU = R_IMUtoACC.transpose(); + Eigen::Matrix3d R_GYROtoIMU = R_IMUtoGYRO.transpose(); // kalibr model: lower triangular of the matrix and R_GYROtoI // rpng model: upper triangular of the matrix and R_ACCtoI From 1d10d1257d7ce68a8d35192e321cb15d504514a4 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 7 Jan 2022 10:39:59 -0500 Subject: [PATCH 27/60] more cleanup and renamings --- config/euroc_mav/kalibr_imu_chain.yaml | 4 +- config/kaist/kalibr_imu_chain.yaml | 4 +- config/kaist_vio/kalibr_imu_chain.yaml | 4 +- config/rpng_aruco/kalibr_imu_chain.yaml | 4 +- config/rpng_ironsides/kalibr_imu_chain.yaml | 4 +- config/rpng_sim/kalibr_imu_chain.yaml | 4 +- config/tum_vi/kalibr_imu_chain.yaml | 4 +- config/uzhfpv_indoor/kalibr_imu_chain.yaml | 4 +- config/uzhfpv_indoor_45/kalibr_imu_chain.yaml | 4 +- ov_msckf/launch/serial.launch | 1 - ov_msckf/launch/simulation.launch | 7 +- ov_msckf/launch/subscribe.launch | 1 - ov_msckf/src/core/VioManagerOptions.h | 4 +- ov_msckf/src/state/Propagator.cpp | 97 ++++++++++--------- ov_msckf/src/state/State.h | 14 ++- 15 files changed, 82 insertions(+), 78 deletions(-) diff --git a/config/euroc_mav/kalibr_imu_chain.yaml b/config/euroc_mav/kalibr_imu_chain.yaml index 09f751ae5..b7e0e6003 100644 --- a/config/euroc_mav/kalibr_imu_chain.yaml +++ b/config/euroc_mav/kalibr_imu_chain.yaml @@ -18,7 +18,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -26,7 +26,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/kaist/kalibr_imu_chain.yaml b/config/kaist/kalibr_imu_chain.yaml index 572575357..3baec9fcd 100644 --- a/config/kaist/kalibr_imu_chain.yaml +++ b/config/kaist/kalibr_imu_chain.yaml @@ -25,7 +25,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -33,7 +33,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/kaist_vio/kalibr_imu_chain.yaml b/config/kaist_vio/kalibr_imu_chain.yaml index ecc3f0b23..ccc96eb70 100644 --- a/config/kaist_vio/kalibr_imu_chain.yaml +++ b/config/kaist_vio/kalibr_imu_chain.yaml @@ -18,7 +18,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -26,7 +26,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/rpng_aruco/kalibr_imu_chain.yaml b/config/rpng_aruco/kalibr_imu_chain.yaml index 3d881e69f..8a4d0bbe8 100644 --- a/config/rpng_aruco/kalibr_imu_chain.yaml +++ b/config/rpng_aruco/kalibr_imu_chain.yaml @@ -18,7 +18,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -26,7 +26,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/rpng_ironsides/kalibr_imu_chain.yaml b/config/rpng_ironsides/kalibr_imu_chain.yaml index 0ab0f2dfc..7168253dd 100644 --- a/config/rpng_ironsides/kalibr_imu_chain.yaml +++ b/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -18,7 +18,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -26,7 +26,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 3d881e69f..8a4d0bbe8 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -18,7 +18,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -26,7 +26,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/tum_vi/kalibr_imu_chain.yaml b/config/tum_vi/kalibr_imu_chain.yaml index 349f66b9a..35a8e844b 100644 --- a/config/tum_vi/kalibr_imu_chain.yaml +++ b/config/tum_vi/kalibr_imu_chain.yaml @@ -18,7 +18,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -26,7 +26,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/config/uzhfpv_indoor/kalibr_imu_chain.yaml index acbd81742..5338cb5c9 100644 --- a/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -22,7 +22,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -30,7 +30,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml index 7125c7678..d02d12239 100644 --- a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -22,7 +22,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoGyro: + R_IMUtoGYRO: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] @@ -30,7 +30,7 @@ imu0: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] - R_ItoAcc: + R_IMUtoACC: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] - [ 0.0, 0.0, 1.0 ] diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index cf1774b1e..3a9f7cc64 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -22,7 +22,6 @@ - diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index c18db0869..c5e57d6cc 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -10,17 +10,15 @@ - + - - @@ -38,12 +36,9 @@ - - - diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 12e4b93a3..114b8e1dd 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -23,7 +23,6 @@ - diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index e4206fe6d..709da0a11 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -304,9 +304,9 @@ struct VioManagerOptions { Eigen::Matrix3d Ta = Eigen::Matrix3d::Identity(); parser->parse_external("relative_config_imu", "imu0", "Ta", Ta); Eigen::Matrix3d R_IMUtoACC = Eigen::Matrix3d::Identity(); - parser->parse_external("relative_config_imu", "imu0", "R_ItoAcc", R_IMUtoACC); + parser->parse_external("relative_config_imu", "imu0", "R_IMUtoACC", R_IMUtoACC); Eigen::Matrix3d R_IMUtoGYRO = Eigen::Matrix3d::Identity(); - parser->parse_external("relative_config_imu", "imu0", "R_ItoGyro", R_IMUtoGYRO); + parser->parse_external("relative_config_imu", "imu0", "R_IMUtoGYRO", R_IMUtoGYRO); Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); parser->parse_external("relative_config_imu", "imu0", "Tg", Tg); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 3a21d4bf1..ae5829f12 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -96,8 +96,6 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest Eigen::Vector3d last_w = Eigen::Vector3d::Zero(); if (!prop_data.empty()) { last_a = state->_calib_imu_ACCtoIMU->Rot() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); - // TODO: is this acceleration used correct? - // Eigen::Vector3d last_aIiinG = state->_imu->Rot().transpose() * last_a - _gravity; last_w = state->_calib_imu_GYROtoIMU->Rot() * state->Dw() * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); } @@ -106,6 +104,7 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest std::vector> Phi_order; Phi_order.push_back(state->_imu); if (state->_options.do_calib_imu_intrinsics) { + assert(false); Phi_order.push_back(state->_calib_imu_dw); Phi_order.push_back(state->_calib_imu_da); if (state->_options.do_calib_imu_g_sensitivity) { @@ -401,7 +400,7 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); } - // Allocate state transition and noise Jacobian + // Allocate state transition and continuous-time noise Jacobian F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, 12); if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { @@ -447,7 +446,7 @@ void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, // Pre-compute things double w_norm = w_hat.norm(); - Eigen::Matrix I_4x4 = Eigen::Matrix::Identity(); + Eigen::Matrix4d I_4x4 = Eigen::Matrix4d::Identity(); Eigen::Matrix3d R_Gtoi = state->_imu->Rot(); // Orientation: Equation (101) and (103) and of Trawny indirect TR @@ -554,11 +553,9 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const // Decompose our angular velocity into a direction and amount double w_norm = w_hat.norm(); double d_th = w_norm * d_t; - Eigen::Vector3d k_hat; - if (w_norm < 1e-8) { - k_hat << 0, 0, 0; - } else { - k_hat << w_hat / w_norm; + Eigen::Vector3d k_hat = Eigen::Vector3d::Zero(); + if (w_norm > 1e-12) { + k_hat = w_hat / w_norm; } // Compute useful identities used throughout @@ -628,7 +625,13 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const } // Store the integrated parameters - Xi_sum << R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; + Xi_sum.setZero(); + Xi_sum.block(0, 0, 3, 3) = R_k1tok; + Xi_sum.block(0, 3, 3, 3) = Xi_1; + Xi_sum.block(0, 6, 3, 3) = Xi_2; + Xi_sum.block(0, 9, 3, 3) = Jr_k1tok; + Xi_sum.block(0, 12, 3, 3) = Xi_3; + Xi_sum.block(0, 15, 3, 3) = Xi_4; } void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, @@ -691,7 +694,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d } } - // This is the change in the orientation from the end of the last prop to the current prop + // The change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information Eigen::Matrix3d R_k = state->_imu->Rot(); Eigen::Vector3d v_k = state->_imu->vel(); @@ -741,46 +744,47 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d F.block(v_id, ba_id, 3, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; F.block(ba_id, ba_id, 3, 3).setIdentity(); - // begin to add the state transition matrix for the omega intrinsics part + // begin to add the state transition matrix for the omega intrinsics Dw part if (Dw_id != -1) { - F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); - F.block(p_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_4 * R_wtoI * compute_H_Dw(state, w_uncorrected); - F.block(v_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_3 * R_wtoI * compute_H_Dw(state, w_uncorrected); + Eigen::MatrixXd H_Dw = compute_H_Dw(state, w_uncorrected); + F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * H_Dw; + F.block(p_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_4 * R_wtoI * H_Dw; + F.block(v_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_3 * R_wtoI * H_Dw; F.block(Dw_id, Dw_id, state->_calib_imu_dw->size(), state->_calib_imu_dw->size()).setIdentity(); } - // begin to add the state transition matrix for the acc intrinsics part + // begin to add the state transition matrix for the acc intrinsics Da part if (Da_id != -1) { + Eigen::MatrixXd H_Da = compute_H_Da(state, a_uncorrected); + F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * H_Da; + F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * H_Da; + F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * H_Da; F.block(Da_id, Da_id, state->_calib_imu_da->size(), state->_calib_imu_da->size()).setIdentity(); - F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * compute_H_Da(state, w_uncorrected); - F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = - R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = - R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * compute_H_Da(state, a_uncorrected); } - // add the state trasition matrix of the tg part + // add the state transition matrix of the Tg part if (Tg_id != -1) { + Eigen::MatrixXd H_Tg = compute_H_Tg(state, a_k); + F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * H_Tg; + F.block(p_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_4 * R_wtoI * Dw * H_Tg; + F.block(v_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_3 * R_wtoI * Dw * H_Tg; F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); - F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); - F.block(p_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_4 * R_wtoI * Dw * compute_H_Tg(state, a_k); - F.block(v_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_3 * R_wtoI * Dw * compute_H_Tg(state, a_k); } - // begin to add the state transition matrix for the acctoI part + // begin to add the state transition matrix for the R_ACCtoIMU part if (th_atoI_id != -1) { - F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); F.block(p_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); + F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); } - // begin to add the state transition matrix for the gyrotoI part + // begin to add the state transition matrix for the R_GYROtoIMU part if (th_wtoI_id != -1) { - F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); F.block(p_id, th_wtoI_id, 3, 3) = -R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); F.block(v_id, th_wtoI_id, 3, 3) = -R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); + F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); } // construct the G part @@ -882,38 +886,41 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // for ba F.block(ba_id, ba_id, 3, 3).setIdentity(); - // begin to add the state transition matrix for the omega intrinsics part + // begin to add the state transition matrix for the omega intrinsics Dw part if (Dw_id != -1) { + Eigen::MatrixXd H_Dw = compute_H_Dw(state, w_uncorrected); + F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * H_Dw; F.block(Dw_id, Dw_id, state->_calib_imu_dw->size(), state->_calib_imu_dw->size()).setIdentity(); - F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * compute_H_Dw(state, w_uncorrected); } - // begin to add the state transition matrix for the acc intrinsics part + // begin to add the state transition matrix for the acc intrinsics Da part if (Da_id != -1) { - F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Tg * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * compute_H_Da(state, a_uncorrected); - F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * dt * R_atoI * compute_H_Da(state, a_uncorrected); + Eigen::MatrixXd H_Da = compute_H_Da(state, a_uncorrected); + F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Tg * R_atoI * H_Da; + F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * H_Da; + F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * dt * R_atoI * H_Da; F.block(Da_id, Da_id, state->_calib_imu_da->size(), state->_calib_imu_da->size()).setIdentity(); } - // begin to add the state transition matrix for the acc intrinsics part + // begin to add the state transition matrix for the gravity sensitivity Tg part + if (Tg_id != -1) { + Eigen::MatrixXd H_Tg = compute_H_Tg(state, a_k); + F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * H_Tg; + F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); + } + + // begin to add the state transition matrix for the R_ACCtoIMU part if (th_atoI_id != -1) { - F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); F.block(p_id, th_atoI_id, 3, 3) = 0.5 * R_k.transpose() * dt * dt * ov_core::skew_x(a_k); F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * dt * ov_core::skew_x(a_k); + F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); } - // begin to add the state transition matrix for the gyro intrinsics part + // begin to add the state transition matrix for the R_GYROtoIMU part if (th_wtoI_id != -1) { - F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); - } - - // begin to add the state transition matrix for the gravity sensitivity part - if (Tg_id != -1) { - F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); - F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * compute_H_Tg(state, a_k); + F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); } // Noise jacobian diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index e0bd79989..c8d66f955 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -80,16 +80,18 @@ class State { /** * @brief Gyroscope intrinsic (scale imperfection and axis misalignment) + * + * If kalibr model, lower triangular of the matrix is used + * If rpng model, upper triangular of the matrix is used + * * @return 3x3 matrix of current imu gyroscope intrinsics */ Eigen::Matrix3d Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); if (_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { - // if kalibr model, lower triangular of the matrix is used Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); } else { - // if rpng model, upper triangular of the matrix is used Dw << _calib_imu_dw->value()(0), _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), _calib_imu_dw->value()(4), 0, 0, _calib_imu_dw->value()(5); } @@ -98,16 +100,18 @@ class State { /** * @brief Accelerometer intrinsic (scale imperfection and axis misalignment) + * + * If kalibr model, lower triangular of the matrix is used + * If rpng model, upper triangular of the matrix is used + * * @return 3x3 matrix of current imu accelerometer intrinsics */ Eigen::Matrix3d Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); if (_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { - // if kalibr model, lower triangular of the matrix is used Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), _calib_imu_da->value()(4), _calib_imu_da->value()(5); } else { - // if rpng model, upper triangular of the matrix is used Da << _calib_imu_da->value()(0), _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), _calib_imu_da->value()(4), 0, 0, _calib_imu_da->value()(5); } @@ -145,7 +149,7 @@ class State { return sz; } - /// Current timestamp (should be the last update time!) + /// Current timestamp (should be the last update time in camera clock frame!) double _timestamp = -1; /// Struct containing filter options From 2f6d3551a7f49eb711330ef413e46d783511669c Mon Sep 17 00:00:00 2001 From: yangyulin Date: Fri, 7 Jan 2022 20:02:38 -0800 Subject: [PATCH 28/60] fix a small bug --- config/rpng_sim/estimator_config.yaml | 6 +++--- config/rpng_sim/kalibr_imu_chain.yaml | 2 +- ov_msckf/launch/simulation.launch | 10 +++++----- ov_msckf/src/state/Propagator.cpp | 15 ++++++++------- 4 files changed, 17 insertions(+), 16 deletions(-) diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index 48d7b3484..a9ddbe607 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -3,12 +3,12 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: false # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) use_analytic_int: true # if analytic integration should be used (overrides imu averaging) -use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints -max_cameras: 2 +use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 1 calib_cam_extrinsics: true calib_cam_intrinsics: true diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 3d881e69f..0f93dd978 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -13,7 +13,7 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + model: "rpng" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index c18db0869..73ba91996 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -10,7 +10,7 @@ - + @@ -24,10 +24,10 @@ - - - - + + + + diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 3a21d4bf1..96909fa1e 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -198,9 +198,9 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times if (state->_options.use_analytic_integration) { predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); } else if (state->_options.use_rk4_integration) { - predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + predict_mean_rk4(state, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p); } else { - predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + predict_mean_discrete(state, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p); } // Now replace imu estimate and fej with propagated values @@ -386,9 +386,8 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Pre-compute some analytical values for the mean and covariance integration Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); - if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { - compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); - } + if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) + compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); // Compute the new state mean value Eigen::Vector4d new_q; @@ -396,9 +395,9 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core if (state->_options.use_analytic_integration) { predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); } else if (state->_options.use_rk4_integration) { - predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + predict_mean_rk4(state, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p); } else { - predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + predict_mean_discrete(state, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p); } // Allocate state transition and noise Jacobian @@ -410,6 +409,8 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core compute_F_and_G_discrete(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, F, G); } + + // Construct our discrete noise covariance matrix // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report From 92eeb9eca2111a82ab87fab46d971c2dd94e13ca Mon Sep 17 00:00:00 2001 From: yangyulin Date: Fri, 7 Jan 2022 20:09:54 -0800 Subject: [PATCH 29/60] as long as turn on the td calibriaton, always consistent --- ov_msckf/src/state/Propagator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 3786a598d..2cc24c78a 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -104,7 +104,6 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest std::vector> Phi_order; Phi_order.push_back(state->_imu); if (state->_options.do_calib_imu_intrinsics) { - assert(false); Phi_order.push_back(state->_calib_imu_dw); Phi_order.push_back(state->_calib_imu_da); if (state->_options.do_calib_imu_g_sensitivity) { From 46131d7ab46bf7c6cfc97bc229a3806fac7f8ae3 Mon Sep 17 00:00:00 2001 From: yangyulin Date: Fri, 7 Jan 2022 20:31:14 -0800 Subject: [PATCH 30/60] fix a bug in ov_eval --- ov_eval/src/calc/ResultSimulation.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 73fe67e36..5823a687f 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -620,11 +620,11 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); matplotlibcpp::title("IMU Dw Error"); - matplotlibcpp::ylabel("dw_3"); - matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("dw_4"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("dw_5"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("dw_6"); matplotlibcpp::xlabel("dataset time (s)"); matplotlibcpp::show(false); @@ -652,11 +652,11 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); matplotlibcpp::title("IMU Da Error"); - matplotlibcpp::ylabel("da_3"); - matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("da_4"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("da_5"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("da_6"); matplotlibcpp::xlabel("dataset time (s)"); matplotlibcpp::show(false); @@ -684,11 +684,11 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_3"); - matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("tg_4"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("tg_5"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_6"); matplotlibcpp::xlabel("dataset time (s)"); matplotlibcpp::show(false); @@ -698,11 +698,11 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_6"); - matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("tg_7"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("tg_8"); + matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::ylabel("tg_9"); matplotlibcpp::xlabel("dataset time (s)"); matplotlibcpp::show(false); From 8069b02af4e04278096fe0438c9cca45a835ff47 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 10 Jan 2022 15:23:52 -0500 Subject: [PATCH 31/60] switch to explictly setting integration method, cleanup parameter storage, revert to using imu noise struct, always do IMU averaging in discrete case --- config/euroc_mav/estimator_config.yaml | 5 +- config/kaist/estimator_config.yaml | 5 +- config/rpng_aruco/estimator_config.yaml | 5 +- config/rpng_ironsides/estimator_config.yaml | 5 +- config/rpng_sim/estimator_config.yaml | 5 +- config/tum_vi/estimator_config.yaml | 5 +- config/uzhfpv_indoor/estimator_config.yaml | 5 +- config/uzhfpv_indoor_45/estimator_config.yaml | 5 +- ov_core/src/utils/sensor_data.h | 54 --------- ov_msckf/launch/simulation.launch | 10 +- ov_msckf/src/core/VioManager.cpp | 32 ++--- ov_msckf/src/core/VioManagerOptions.h | 91 ++++++++------ ov_msckf/src/ros/RosVisualizerHelper.h | 38 +++--- ov_msckf/src/sim/Simulator.cpp | 59 +++++---- ov_msckf/src/state/Propagator.cpp | 114 ++++++++---------- ov_msckf/src/state/Propagator.h | 44 +++++-- ov_msckf/src/state/State.cpp | 38 +++--- ov_msckf/src/state/State.h | 4 +- ov_msckf/src/state/StateOptions.h | 45 ++++--- ov_msckf/src/update/UpdaterZeroVelocity.cpp | 10 +- ov_msckf/src/update/UpdaterZeroVelocity.h | 6 +- 21 files changed, 266 insertions(+), 319 deletions(-) diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index 12d7a1329..ec2995ccb 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: true # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) diff --git a/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml index cf2779a7a..5962c376e 100644 --- a/config/kaist/estimator_config.yaml +++ b/config/kaist/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml index 650bdbb5d..d8b75a7d8 100644 --- a/config/rpng_aruco/estimator_config.yaml +++ b/config/rpng_aruco/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml index 10fc64c1b..c485f9b0b 100644 --- a/config/rpng_ironsides/estimator_config.yaml +++ b/config/rpng_ironsides/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index 48d7b3484..dbcfcff50 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: false # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "analytical" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml index 19d169589..457ea7b08 100644 --- a/config/tum_vi/estimator_config.yaml +++ b/config/tum_vi/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml index be9bde714..5516197a7 100644 --- a/config/uzhfpv_indoor/estimator_config.yaml +++ b/config/uzhfpv_indoor/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml index be9bde714..5516197a7 100644 --- a/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/config/uzhfpv_indoor_45/estimator_config.yaml @@ -3,10 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4_int: false # if rk4 integration should be used (overrides imu averaging) -use_analytic_int: true # if analytic integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index d0a6bf7b9..7f3fc2d4e 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -80,60 +80,6 @@ struct CameraData { } }; -/** - * @brief Struct of our imu noise and intrinsic parameters. - * - * For details on each specific model see @ref imu_intrinsics - */ -struct ImuConfig { - - /** - * @brief What model our IMU is - */ - enum ImuModel { - KALIBR, - RPNG - }; - - /// IMU model our intrinsic calibration is using. - ImuModel imu_model = ImuModel::KALIBR; - - /// Gyroscope IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse) - Eigen::Matrix vec_dw; - - /// Accelerometer IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse) - Eigen::Matrix vec_da; - - /// Gyroscope gravity sensitivity (scale imperfection and axis misalignment, column-wise) - Eigen::Matrix vec_tg; - - /// Rotation from gyroscope frame to the "IMU" accelerometer frame - Eigen::Matrix q_ACCtoIMU; - - /// Rotation from accelerometer to the "IMU" gyroscope frame frame - Eigen::Matrix q_GYROtoIMU; - - /// Gyroscope white noise (rad/s/sqrt(hz)) - double sigma_w = 1.6968e-04; - - /// Gyroscope random walk (rad/s^2/sqrt(hz)) - double sigma_wb = 1.9393e-05; - - /// Accelerometer white noise (m/s^2/sqrt(hz)) - double sigma_a = 2.0000e-3; - - /// Accelerometer random walk (m/s^3/sqrt(hz)) - double sigma_ab = 3.0000e-03; - - /// Nice print function of what parameters we have loaded - void print() { - PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); - PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); - PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); - PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); - } -}; - } // namespace ov_core #endif // OV_CORE_SENSOR_DATA_H \ No newline at end of file diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index c5e57d6cc..665724195 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -5,7 +5,7 @@ - + @@ -22,10 +22,10 @@ - - - - + + + + diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index cd1e75b4a..625dda871 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -48,16 +48,16 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { state = std::make_shared(params.state_options); // Set the IMU intrinsics - state->_calib_imu_dw->set_value(params.imu_config.vec_dw); - state->_calib_imu_dw->set_fej(params.imu_config.vec_dw); - state->_calib_imu_da->set_value(params.imu_config.vec_da); - state->_calib_imu_da->set_fej(params.imu_config.vec_da); - state->_calib_imu_tg->set_value(params.imu_config.vec_tg); - state->_calib_imu_tg->set_fej(params.imu_config.vec_tg); - state->_calib_imu_GYROtoIMU->set_value(params.imu_config.q_GYROtoIMU); - state->_calib_imu_GYROtoIMU->set_fej(params.imu_config.q_GYROtoIMU); - state->_calib_imu_ACCtoIMU->set_value(params.imu_config.q_ACCtoIMU); - state->_calib_imu_ACCtoIMU->set_fej(params.imu_config.q_ACCtoIMU); + state->_calib_imu_dw->set_value(params.vec_dw); + state->_calib_imu_dw->set_fej(params.vec_dw); + state->_calib_imu_da->set_value(params.vec_da); + state->_calib_imu_da->set_fej(params.vec_da); + state->_calib_imu_tg->set_value(params.vec_tg); + state->_calib_imu_tg->set_fej(params.vec_tg); + state->_calib_imu_GYROtoIMU->set_value(params.q_GYROtoIMU); + state->_calib_imu_GYROtoIMU->set_fej(params.q_GYROtoIMU); + state->_calib_imu_ACCtoIMU->set_value(params.q_ACCtoIMU); + state->_calib_imu_ACCtoIMU->set_fej(params.q_ACCtoIMU); // Timeoffset from camera to IMU Eigen::VectorXd temp_camimu_dt; @@ -123,7 +123,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { } // Initialize our state propagator - propagator = std::make_shared(params.imu_config, params.gravity_mag); + propagator = std::make_shared(params.imu_noises, params.gravity_mag); // Our state initialize initializer = std::make_shared(params.init_options, trackFEATS->get_feature_database()); @@ -134,7 +134,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // If we are using zero velocity updates, then create the updater if (params.try_zupt) { - updaterZUPT = std::make_shared(params.zupt_options, params.imu_config, trackFEATS->get_feature_database(), + updaterZUPT = std::make_shared(params.zupt_options, params.imu_noises, trackFEATS->get_feature_database(), propagator, params.gravity_mag, params.zupt_max_velocity, params.zupt_noise_multiplier, params.zupt_max_disparity); } @@ -729,7 +729,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) } // Debug for imu intrinsics - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::KALIBR) { PRINT_INFO("Dw = | %.4f,%.4f,%.4f | %.4f,%.4f | %.4f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); @@ -737,7 +737,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) state->_calib_imu_da->value()(2), state->_calib_imu_da->value()(3), state->_calib_imu_da->value()(4), state->_calib_imu_da->value()(5)); } - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::RPNG) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::RPNG) { PRINT_INFO("Dw = | %.4f | %.4f,%.4f | %.4f,%.4f,%.4f |\n", state->_calib_imu_dw->value()(0), state->_calib_imu_dw->value()(1), state->_calib_imu_dw->value()(2), state->_calib_imu_dw->value()(3), state->_calib_imu_dw->value()(4), state->_calib_imu_dw->value()(5)); @@ -751,11 +751,11 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), state->_calib_imu_tg->value()(6), state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8)); } - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::KALIBR) { PRINT_INFO("q_GYROtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_GYROtoIMU->value()(0), state->_calib_imu_GYROtoIMU->value()(1), state->_calib_imu_GYROtoIMU->value()(2), state->_calib_imu_GYROtoIMU->value()(3)); } - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ImuConfig::ImuModel::RPNG) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::RPNG) { PRINT_INFO("q_ACCtoI = %.3f,%.3f,%.3f,%.3f\n", state->_calib_imu_ACCtoIMU->value()(0), state->_calib_imu_ACCtoIMU->value()(1), state->_calib_imu_ACCtoIMU->value()(2), state->_calib_imu_ACCtoIMU->value()(3)); } diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 709da0a11..e6eec822a 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -60,9 +60,11 @@ struct VioManagerOptions { */ void print_and_load(const std::shared_ptr &parser = nullptr) { print_and_load_estimator(parser); + print_and_load_trackers(parser); print_and_load_noise(parser); + + // needs to be called last print_and_load_state(parser); - print_and_load_trackers(parser); } // ESTIMATOR =============================== @@ -129,8 +131,8 @@ struct VioManagerOptions { // NOISE / CHI2 ============================ - /// IMU noise (gyroscope and accelerometer) - ov_core::ImuConfig imu_config; + /// Continuous-time IMU noise (gyroscope and accelerometer) + Propagator::NoiseManager imu_noises; /// Update options for MSCKF features (pixel noise and chi2 multiplier) UpdaterOptions msckf_options; @@ -153,12 +155,12 @@ struct VioManagerOptions { void print_and_load_noise(const std::shared_ptr &parser = nullptr) { PRINT_DEBUG("NOISE PARAMETERS:\n"); if (parser != nullptr) { - parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", imu_config.sigma_w); - parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_config.sigma_wb); - parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_config.sigma_a); - parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_config.sigma_ab); + parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", imu_noises.sigma_w); + parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_noises.sigma_wb); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_noises.sigma_a); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_noises.sigma_ab); } - imu_config.print(); + imu_noises.print(); if (parser != nullptr) { parser->parse_config("up_msckf_sigma_px", msckf_options.sigma_pix); parser->parse_config("up_msckf_chi2_multipler", msckf_options.chi2_multipler); @@ -186,6 +188,21 @@ struct VioManagerOptions { /// Gravity magnitude in the global frame (i.e. should be 9.81 typically) double gravity_mag = 9.81; + /// Gyroscope IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse) + Eigen::Matrix vec_dw; + + /// Accelerometer IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse) + Eigen::Matrix vec_da; + + /// Gyroscope gravity sensitivity (scale imperfection and axis misalignment, column-wise) + Eigen::Matrix vec_tg; + + /// Rotation from gyroscope frame to the "IMU" accelerometer frame + Eigen::Matrix q_ACCtoIMU; + + /// Rotation from accelerometer to the "IMU" gyroscope frame frame + Eigen::Matrix q_GYROtoIMU; + /// Time offset between camera and IMU. double calib_camimu_dt = 0.0; @@ -210,8 +227,6 @@ struct VioManagerOptions { void print_and_load_state(const std::shared_ptr &parser = nullptr) { if (parser != nullptr) { parser->parse_config("gravity_mag", gravity_mag); - parser->parse_config("max_cameras", state_options.num_cameras); // might be redundant - parser->parse_config("downsample_cameras", downsample_cameras); // might be redundant for (int i = 0; i < state_options.num_cameras; i++) { // Time offset (use the first one) @@ -286,18 +301,6 @@ struct VioManagerOptions { } } - // IMU model - std::string imu_model_str = "kalibr"; - parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); // might be redundant - if (imu_model_str == "kalibr" || imu_model_str == "calibrated") { - imu_config.imu_model = ov_core::ImuConfig::ImuModel::KALIBR; - } else if (imu_model_str == "rpng") { - imu_config.imu_model = ov_core::ImuConfig::ImuModel::RPNG; - } else { - PRINT_ERROR(RED "VioManager(): invalid imu model: %s\n" RESET, imu_model_str.c_str()); - std::exit(EXIT_FAILURE); - } - // IMU intrinsics Eigen::Matrix3d Tw = Eigen::Matrix3d::Identity(); parser->parse_external("relative_config_imu", "imu0", "Tw", Tw); @@ -316,19 +319,35 @@ struct VioManagerOptions { Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d R_ACCtoIMU = R_IMUtoACC.transpose(); Eigen::Matrix3d R_GYROtoIMU = R_IMUtoGYRO.transpose(); + if (std::isnan(Tw.norm()) || std::isnan(Dw.norm())) { + std::stringstream ss; + ss << "gyroscope has bad intrinsic values!" << std::endl; + ss << "Tw - " << std::endl << Tw << std::endl << std::endl; + ss << "Dw - " << std::endl << Dw << std::endl << std::endl; + PRINT_DEBUG(RED "" RESET, ss.str().c_str()); + std::exit(EXIT_FAILURE); + } + if (std::isnan(Ta.norm()) || std::isnan(Da.norm())) { + std::stringstream ss; + ss << "accelerometer has bad intrinsic values!" << std::endl; + ss << "Ta - " << std::endl << Ta << std::endl << std::endl; + ss << "Da - " << std::endl << Da << std::endl << std::endl; + PRINT_DEBUG(RED "" RESET, ss.str().c_str()); + std::exit(EXIT_FAILURE); + } // kalibr model: lower triangular of the matrix and R_GYROtoI // rpng model: upper triangular of the matrix and R_ACCtoI - if (imu_config.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { - imu_config.vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); - imu_config.vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); + if (state_options.imu_model == StateOptions::ImuModel::KALIBR) { + vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2); + vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2); } else { - imu_config.vec_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2); - imu_config.vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); + vec_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2); + vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2); } - imu_config.vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2); - imu_config.q_GYROtoIMU = ov_core::rot_2_quat(R_GYROtoIMU); - imu_config.q_ACCtoIMU = ov_core::rot_2_quat(R_ACCtoIMU); + vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2); + q_GYROtoIMU = ov_core::rot_2_quat(R_GYROtoIMU); + q_ACCtoIMU = ov_core::rot_2_quat(R_ACCtoIMU); } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); @@ -362,12 +381,12 @@ struct VioManagerOptions { } PRINT_DEBUG("IMU PARAMETERS:\n"); std::stringstream ss; - ss << "imu model:" << ((imu_config.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) ? "kalibr" : "rpng") << std::endl; - ss << "Dw (columnwise):" << imu_config.vec_dw.transpose() << std::endl; - ss << "Da (columnwise):" << imu_config.vec_da.transpose() << std::endl; - ss << "Tg (columnwise):" << imu_config.vec_tg.transpose() << std::endl; - ss << "q_GYROtoI: " << imu_config.q_GYROtoIMU.transpose() << std::endl; - ss << "q_ACCtoI: " << imu_config.q_ACCtoIMU.transpose() << std::endl; + ss << "imu model:" << ((state_options.imu_model == StateOptions::ImuModel::KALIBR) ? "kalibr" : "rpng") << std::endl; + ss << "Dw (columnwise):" << vec_dw.transpose() << std::endl; + ss << "Da (columnwise):" << vec_da.transpose() << std::endl; + ss << "Tg (columnwise):" << vec_tg.transpose() << std::endl; + ss << "q_GYROtoI: " << q_GYROtoIMU.transpose() << std::endl; + ss << "q_ACCtoI: " << q_ACCtoIMU.transpose() << std::endl; PRINT_DEBUG(ss.str().c_str()); } diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 14aac043b..0f6c2c5c9 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -283,26 +283,24 @@ class RosVisualizerHelper { // Get the base IMU information of_state_gt.precision(0); - of_state_gt << sim->get_true_parameters().imu_config.imu_model << " "; + of_state_gt << sim->get_true_parameters().state_options.imu_model << " "; of_state_gt.precision(8); - of_state_gt << sim->get_true_parameters().imu_config.vec_dw(0) << " " << sim->get_true_parameters().imu_config.vec_dw(1) << " " - << sim->get_true_parameters().imu_config.vec_dw(2) << " " << sim->get_true_parameters().imu_config.vec_dw(3) << " " - << sim->get_true_parameters().imu_config.vec_dw(4) << " " << sim->get_true_parameters().imu_config.vec_dw(5) << " "; - of_state_gt << sim->get_true_parameters().imu_config.vec_da(0) << " " << sim->get_true_parameters().imu_config.vec_da(1) << " " - << sim->get_true_parameters().imu_config.vec_da(2) << " " << sim->get_true_parameters().imu_config.vec_da(3) << " " - << sim->get_true_parameters().imu_config.vec_da(4) << " " << sim->get_true_parameters().imu_config.vec_da(5) << " "; - of_state_gt << sim->get_true_parameters().imu_config.vec_tg(0) << " " << sim->get_true_parameters().imu_config.vec_tg(1) << " " - << sim->get_true_parameters().imu_config.vec_tg(2) << " " << sim->get_true_parameters().imu_config.vec_tg(3) << " " - << sim->get_true_parameters().imu_config.vec_tg(4) << " " << sim->get_true_parameters().imu_config.vec_tg(5) << " " - << sim->get_true_parameters().imu_config.vec_tg(6) << " " << sim->get_true_parameters().imu_config.vec_tg(7) << " " - << sim->get_true_parameters().imu_config.vec_tg(8) << " "; - of_state_gt << sim->get_true_parameters().imu_config.q_GYROtoIMU(0) << " " << sim->get_true_parameters().imu_config.q_GYROtoIMU(1) - << " " << sim->get_true_parameters().imu_config.q_GYROtoIMU(2) << " " - << sim->get_true_parameters().imu_config.q_GYROtoIMU(3) << " "; - of_state_gt << sim->get_true_parameters().imu_config.q_ACCtoIMU(0) << " " << sim->get_true_parameters().imu_config.q_ACCtoIMU(1) - << " " << sim->get_true_parameters().imu_config.q_ACCtoIMU(2) << " " - << sim->get_true_parameters().imu_config.q_ACCtoIMU(3) << " "; + of_state_gt << sim->get_true_parameters().vec_dw(0) << " " << sim->get_true_parameters().vec_dw(1) << " " + << sim->get_true_parameters().vec_dw(2) << " " << sim->get_true_parameters().vec_dw(3) << " " + << sim->get_true_parameters().vec_dw(4) << " " << sim->get_true_parameters().vec_dw(5) << " "; + of_state_gt << sim->get_true_parameters().vec_da(0) << " " << sim->get_true_parameters().vec_da(1) << " " + << sim->get_true_parameters().vec_da(2) << " " << sim->get_true_parameters().vec_da(3) << " " + << sim->get_true_parameters().vec_da(4) << " " << sim->get_true_parameters().vec_da(5) << " "; + of_state_gt << sim->get_true_parameters().vec_tg(0) << " " << sim->get_true_parameters().vec_tg(1) << " " + << sim->get_true_parameters().vec_tg(2) << " " << sim->get_true_parameters().vec_tg(3) << " " + << sim->get_true_parameters().vec_tg(4) << " " << sim->get_true_parameters().vec_tg(5) << " " + << sim->get_true_parameters().vec_tg(6) << " " << sim->get_true_parameters().vec_tg(7) << " " + << sim->get_true_parameters().vec_tg(8) << " "; + of_state_gt << sim->get_true_parameters().q_GYROtoIMU(0) << " " << sim->get_true_parameters().q_GYROtoIMU(1) << " " + << sim->get_true_parameters().q_GYROtoIMU(2) << " " << sim->get_true_parameters().q_GYROtoIMU(3) << " "; + of_state_gt << sim->get_true_parameters().q_ACCtoIMU(0) << " " << sim->get_true_parameters().q_ACCtoIMU(1) << " " + << sim->get_true_parameters().q_ACCtoIMU(2) << " " << sim->get_true_parameters().q_ACCtoIMU(3) << " "; // New line of_state_gt << endl; } @@ -452,7 +450,7 @@ class RosVisualizerHelper { // imu intrinsics: kalibr R_gyrotoI of_state_est << state->_calib_imu_GYROtoIMU->value()(0) << " " << state->_calib_imu_GYROtoIMU->value()(1) << " " << state->_calib_imu_GYROtoIMU->value()(2) << " " << state->_calib_imu_GYROtoIMU->value()(3) << " "; - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::KALIBR) { int index_wtoI = state->_calib_imu_GYROtoIMU->id(); of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " " << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " "; @@ -463,7 +461,7 @@ class RosVisualizerHelper { // imu intrinsics: rpng R_acctoI of_state_est << state->_calib_imu_ACCtoIMU->value()(0) << " " << state->_calib_imu_ACCtoIMU->value()(1) << " " << state->_calib_imu_ACCtoIMU->value()(2) << " " << state->_calib_imu_ACCtoIMU->value()(3) << " "; - if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == ov_core::ImuConfig::ImuModel::RPNG) { + if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::RPNG) { int index_atoI = state->_calib_imu_ACCtoIMU->id(); of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " " << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " "; diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 9b83542a9..b0a297522 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -230,24 +230,24 @@ void Simulator::perturb_parameters(std::mt19937 gen_state, VioManagerOptions &pa // If we need to perturb the imu intrinsics if (params_.state_options.do_calib_imu_intrinsics) { for (int j = 0; j < 6; j++) { - params_.imu_config.vec_dw(j) += 0.004 * w(gen_state); - params_.imu_config.vec_da(j) += 0.004 * w(gen_state); + params_.vec_dw(j) += 0.004 * w(gen_state); + params_.vec_da(j) += 0.004 * w(gen_state); } - if (params_.state_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (params_.state_options.imu_model == StateOptions::ImuModel::KALIBR) { Eigen::Vector3d w_vec; w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); - params_.imu_config.q_GYROtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.q_GYROtoIMU)); + params_.q_GYROtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.q_GYROtoIMU)); } else { Eigen::Vector3d w_vec; w_vec << 0.002 * w(gen_state), 0.002 * w(gen_state), 0.002 * w(gen_state); - params_.imu_config.q_ACCtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.imu_config.q_ACCtoIMU)); + params_.q_ACCtoIMU = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.q_ACCtoIMU)); } } // If we need to perturb gravity sensitivity if (params_.state_options.do_calib_imu_g_sensitivity) { for (int j = 0; j < 9; j++) { - params_.imu_config.vec_tg(j) += 0.004 * w(gen_state); + params_.vec_tg(j) += 0.004 * w(gen_state); } } } @@ -335,45 +335,40 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto // - rpng: upper triangular of the matrix is used Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (params.imu_config.imu_model == ImuConfig::ImuModel::KALIBR) { - Dw << params.imu_config.vec_dw(0), 0, 0, params.imu_config.vec_dw(1), params.imu_config.vec_dw(3), 0, params.imu_config.vec_dw(2), - params.imu_config.vec_dw(4), params.imu_config.vec_dw(5); - Da << params.imu_config.vec_da(0), 0, 0, params.imu_config.vec_da(1), params.imu_config.vec_da(3), 0, params.imu_config.vec_da(2), - params.imu_config.vec_da(4), params.imu_config.vec_da(5); + if (params.state_options.imu_model == StateOptions::ImuModel::KALIBR) { + Dw << params.vec_dw(0), 0, 0, params.vec_dw(1), params.vec_dw(3), 0, params.vec_dw(2), params.vec_dw(4), params.vec_dw(5); + Da << params.vec_da(0), 0, 0, params.vec_da(1), params.vec_da(3), 0, params.vec_da(2), params.vec_da(4), params.vec_da(5); } else { - Dw << params.imu_config.vec_dw(0), params.imu_config.vec_dw(1), params.imu_config.vec_dw(3), 0, params.imu_config.vec_dw(2), - params.imu_config.vec_dw(4), 0, 0, params.imu_config.vec_dw(5); - Da << params.imu_config.vec_da(0), params.imu_config.vec_da(1), params.imu_config.vec_da(3), 0, params.imu_config.vec_da(2), - params.imu_config.vec_da(4), 0, 0, params.imu_config.vec_da(5); + Dw << params.vec_dw(0), params.vec_dw(1), params.vec_dw(3), 0, params.vec_dw(2), params.vec_dw(4), 0, 0, params.vec_dw(5); + Da << params.vec_da(0), params.vec_da(1), params.vec_da(3), 0, params.vec_da(2), params.vec_da(4), 0, 0, params.vec_da(5); } Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << params.imu_config.vec_tg(0), params.imu_config.vec_tg(3), params.imu_config.vec_tg(6), params.imu_config.vec_tg(1), - params.imu_config.vec_tg(4), params.imu_config.vec_tg(7), params.imu_config.vec_tg(2), params.imu_config.vec_tg(5), - params.imu_config.vec_tg(8); + Tg << params.vec_tg(0), params.vec_tg(3), params.vec_tg(6), params.vec_tg(1), params.vec_tg(4), params.vec_tg(7), params.vec_tg(2), + params.vec_tg(5), params.vec_tg(8); // Get the readings with the imu intrinsic "distortion" Eigen::Matrix3d Tw = Dw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); Eigen::Matrix3d Ta = Da.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); - Eigen::Vector3d omega_inGYRO = Tw * quat_2_Rot(params.imu_config.q_GYROtoIMU).transpose() * omega_inI + Tg * accel_inI; - Eigen::Vector3d accel_inACC = Ta * quat_2_Rot(params.imu_config.q_ACCtoIMU).transpose() * accel_inI; + Eigen::Vector3d omega_inGYRO = Tw * quat_2_Rot(params.q_GYROtoIMU).transpose() * omega_inI + Tg * accel_inI; + Eigen::Vector3d accel_inACC = Ta * quat_2_Rot(params.q_ACCtoIMU).transpose() * accel_inI; // Now add noise to these measurements double dt = 1.0 / params.sim_freq_imu; std::normal_distribution w(0, 1); - wm(0) = omega_inGYRO(0) + true_bias_gyro(0) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(1) = omega_inGYRO(1) + true_bias_gyro(1) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - wm(2) = omega_inGYRO(2) + true_bias_gyro(2) + params.imu_config.sigma_w / std::sqrt(dt) * w(gen_meas_imu); - am(0) = accel_inACC(0) + true_bias_accel(0) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(1) = accel_inACC(1) + true_bias_accel(1) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - am(2) = accel_inACC(2) + true_bias_accel(2) + params.imu_config.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + wm(0) = omega_inGYRO(0) + true_bias_gyro(0) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(1) = omega_inGYRO(1) + true_bias_gyro(1) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + wm(2) = omega_inGYRO(2) + true_bias_gyro(2) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); + am(0) = accel_inACC(0) + true_bias_accel(0) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(1) = accel_inACC(1) + true_bias_accel(1) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); + am(2) = accel_inACC(2) + true_bias_accel(2) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); // Move the biases forward in time - true_bias_gyro(0) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_gyro(1) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_gyro(2) += params.imu_config.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(0) += params.imu_config.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(1) += params.imu_config.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(2) += params.imu_config.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(0) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(1) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(2) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(0) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(1) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(2) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); // Append the current true bias to our history hist_true_bias_time.push_back(timestamp_last_imu); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index ae5829f12..b79da380d 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -104,13 +104,12 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest std::vector> Phi_order; Phi_order.push_back(state->_imu); if (state->_options.do_calib_imu_intrinsics) { - assert(false); Phi_order.push_back(state->_calib_imu_dw); Phi_order.push_back(state->_calib_imu_da); if (state->_options.do_calib_imu_g_sensitivity) { Phi_order.push_back(state->_calib_imu_tg); } - if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.imu_model == StateOptions::ImuModel::KALIBR) { Phi_order.push_back(state->_calib_imu_GYROtoIMU); } else { Phi_order.push_back(state->_calib_imu_ACCtoIMU); @@ -160,46 +159,40 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // Corrected imu acc measurements with our current biases Eigen::Vector3d a_hat1 = data_minus.am - state->_imu->bias_a(); Eigen::Vector3d a_hat2 = data_plus.am - state->_imu->bias_a(); - Eigen::Vector3d a_hat = a_hat1; - if (state->_options.imu_avg) { - a_hat = .5 * (a_hat1 + a_hat2); - } + Eigen::Vector3d a_hat_avg = .5 * (a_hat1 + a_hat2); // Correct imu readings with IMU intrinsics Eigen::Matrix3d R_ACCtoIMU = state->_calib_imu_ACCtoIMU->Rot(); - a_hat = R_ACCtoIMU * state->Da() * a_hat; a_hat1 = R_ACCtoIMU * state->Da() * a_hat1; a_hat2 = R_ACCtoIMU * state->Da() * a_hat2; + a_hat_avg = R_ACCtoIMU * state->Da() * a_hat_avg; // Corrected imu gyro measurements with our current biases Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - state->Tg() * a_hat1; Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - state->Tg() * a_hat2; - Eigen::Vector3d w_hat = w_hat1; - if (state->_options.imu_avg) { - w_hat = .5 * (w_hat1 + w_hat2); - } + Eigen::Vector3d w_hat_avg = .5 * (w_hat1 + w_hat2); // Correct imu readings with IMU intrinsics Eigen::Matrix3d R_GYROtoIMU = state->_calib_imu_GYROtoIMU->Rot(); - w_hat = R_GYROtoIMU * state->Dw() * w_hat; w_hat1 = R_GYROtoIMU * state->Dw() * w_hat1; w_hat2 = R_GYROtoIMU * state->Dw() * w_hat2; + w_hat_avg = R_GYROtoIMU * state->Dw() * w_hat_avg; // Pre-compute some analytical values Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); - if (state->_options.use_analytic_integration) { - compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); + if (state->_options.integration_method == StateOptions::ANALYTICAL) { + compute_Xi_sum(state, dt, w_hat_avg, a_hat_avg, Xi_sum); } // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if (state->_options.use_analytic_integration) { - predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); - } else if (state->_options.use_rk4_integration) { - predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + if (state->_options.integration_method == StateOptions::ANALYTICAL) { + predict_mean_analytic(state, dt, w_hat_avg, a_hat_avg, new_q, new_v, new_p, Xi_sum); + } else if (state->_options.integration_method == StateOptions::RK4) { + predict_mean_rk4(state, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p); } else { - predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + predict_mean_discrete(state, dt, w_hat_avg, a_hat_avg, new_q, new_v, new_p); } // Now replace imu estimate and fej with propagated values @@ -356,67 +349,63 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Corrected imu acc measurements with our current biases Eigen::Vector3d a_hat1 = data_minus.am - state->_imu->bias_a(); Eigen::Vector3d a_hat2 = data_plus.am - state->_imu->bias_a(); - Eigen::Vector3d a_hat = a_hat1; - if (state->_options.imu_avg) { - a_hat = .5 * (a_hat1 + a_hat2); - } + Eigen::Vector3d a_hat_avg = .5 * (a_hat1 + a_hat2); // Convert "raw" imu to its corrected frame using the IMU intrinsics - Eigen::Vector3d a_uncorrected = a_hat; + Eigen::Vector3d a_uncorrected = a_hat_avg; Eigen::Matrix3d R_ACCtoIMU = state->_calib_imu_ACCtoIMU->Rot(); - a_hat = R_ACCtoIMU * state->Da() * a_hat; a_hat1 = R_ACCtoIMU * state->Da() * a_hat1; a_hat2 = R_ACCtoIMU * state->Da() * a_hat2; + a_hat_avg = R_ACCtoIMU * state->Da() * a_hat_avg; // Corrected imu gyro measurements with our current biases and gravity sensitivity Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - state->Tg() * a_hat1; Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - state->Tg() * a_hat2; - Eigen::Vector3d w_hat = w_hat1; - if (state->_options.imu_avg) { - w_hat = .5 * (w_hat1 + w_hat2); - } + Eigen::Vector3d w_hat_avg = .5 * (w_hat1 + w_hat2); // Convert "raw" imu to its corrected frame using the IMU intrinsics - Eigen::Vector3d w_uncorrected = w_hat; + Eigen::Vector3d w_uncorrected = w_hat_avg; Eigen::Matrix3d R_GYROtoIMU = state->_calib_imu_GYROtoIMU->Rot(); - w_hat = R_GYROtoIMU * state->Dw() * w_hat; w_hat1 = R_GYROtoIMU * state->Dw() * w_hat1; w_hat2 = R_GYROtoIMU * state->Dw() * w_hat2; + w_hat_avg = R_GYROtoIMU * state->Dw() * w_hat_avg; // Pre-compute some analytical values for the mean and covariance integration Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); - if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { - compute_Xi_sum(state, dt, w_hat, a_hat, Xi_sum); + if (state->_options.integration_method == StateOptions::IntegrationMethod::RK4 || + state->_options.integration_method == StateOptions::IntegrationMethod::ANALYTICAL) { + compute_Xi_sum(state, dt, w_hat_avg, a_hat_avg, Xi_sum); } // Compute the new state mean value Eigen::Vector4d new_q; Eigen::Vector3d new_v, new_p; - if (state->_options.use_analytic_integration) { - predict_mean_analytic(state, dt, w_hat, a_hat, new_q, new_v, new_p, Xi_sum); - } else if (state->_options.use_rk4_integration) { - predict_mean_rk4(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + if (state->_options.integration_method == StateOptions::IntegrationMethod::ANALYTICAL) { + predict_mean_analytic(state, dt, w_hat_avg, a_hat_avg, new_q, new_v, new_p, Xi_sum); + } else if (state->_options.integration_method == StateOptions::IntegrationMethod::RK4) { + predict_mean_rk4(state, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p); } else { - predict_mean_discrete(state, dt, w_hat, a_hat, w_hat2, a_hat2, new_q, new_v, new_p); + predict_mean_discrete(state, dt, w_hat_avg, a_hat_avg, new_q, new_v, new_p); } // Allocate state transition and continuous-time noise Jacobian F = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, 12); - if (state->_options.use_analytic_integration || state->_options.use_rk4_integration) { - compute_F_and_G_analytic(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G); + if (state->_options.integration_method == StateOptions::IntegrationMethod::RK4 || + state->_options.integration_method == StateOptions::IntegrationMethod::ANALYTICAL) { + compute_F_and_G_analytic(state, dt, w_hat_avg, a_hat_avg, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G); } else { - compute_F_and_G_discrete(state, dt, w_hat, a_hat, w_uncorrected, a_uncorrected, new_q, new_v, new_p, F, G); + compute_F_and_G_discrete(state, dt, w_hat_avg, a_hat_avg, w_uncorrected, a_uncorrected, new_q, new_v, new_p, F, G); } // Construct our discrete noise covariance matrix // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report Eigen::Matrix Qc = Eigen::Matrix::Zero(); - Qc.block(0, 0, 3, 3) = std::pow(_imu_config.sigma_w, 2) / dt * Eigen::Matrix3d::Identity(); - Qc.block(3, 3, 3, 3) = std::pow(_imu_config.sigma_a, 2) / dt * Eigen::Matrix3d::Identity(); - Qc.block(6, 6, 3, 3) = std::pow(_imu_config.sigma_wb, 2) / dt * Eigen::Matrix3d::Identity(); - Qc.block(9, 9, 3, 3) = std::pow(_imu_config.sigma_ab, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(0, 0, 3, 3) = std::pow(_noises.sigma_w, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(3, 3, 3, 3) = std::pow(_noises.sigma_a, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(6, 6, 3, 3) = std::pow(_noises.sigma_wb, 2) / dt * Eigen::Matrix3d::Identity(); + Qc.block(9, 9, 3, 3) = std::pow(_noises.sigma_ab, 2) / dt * Eigen::Matrix3d::Identity(); // Compute the noise injected into the state over the interval Qd = Eigen::MatrixXd::Zero(state->imu_intrinsic_size() + 15, state->imu_intrinsic_size() + 15); @@ -432,18 +421,9 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core state->_imu->set_fej(imu_x); } -void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, - const Eigen::Vector3d &a_hat1, const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, +void Propagator::predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) { - // If we are averaging the IMU, then do so - Eigen::Vector3d w_hat = w_hat1; - Eigen::Vector3d a_hat = a_hat1; - if (state->_options.imu_avg) { - w_hat = .5 * (w_hat1 + w_hat2); - a_hat = .5 * (a_hat1 + a_hat2); - } - // Pre-compute things double w_norm = w_hat.norm(); Eigen::Matrix4d I_4x4 = Eigen::Matrix4d::Identity(); @@ -547,12 +527,12 @@ void Propagator::predict_mean_rk4(std::shared_ptr state, double dt, const new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v; } -void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, +void Propagator::compute_Xi_sum(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Matrix &Xi_sum) { // Decompose our angular velocity into a direction and amount double w_norm = w_hat.norm(); - double d_th = w_norm * d_t; + double d_th = w_norm * dt; Eigen::Vector3d k_hat = Eigen::Vector3d::Zero(); if (w_norm > 1e-12) { k_hat = w_hat / w_norm; @@ -560,8 +540,8 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const // Compute useful identities used throughout Eigen::Matrix3d I_3x3 = Eigen::Matrix3d::Identity(); - double d_t2 = std::pow(d_t, 2); - double d_t3 = std::pow(d_t, 3); + double d_t2 = std::pow(dt, 2); + double d_t3 = std::pow(dt, 3); double w_norm2 = std::pow(w_norm, 2); double w_norm3 = std::pow(w_norm, 3); double cos_dth = std::cos(d_th); @@ -591,7 +571,7 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const if (!small_w) { // first order rotation integration with constant omega - Xi_1 = I_3x3 * d_t + (1.0 - cos_dth) / w_norm * sK + (d_t - sin_dth / w_norm) * sK2; + Xi_1 = I_3x3 * dt + (1.0 - cos_dth) / w_norm * sK + (dt - sin_dth / w_norm) * sK2; // second order rotation integration with constant omega Xi_2 = 1.0 / 2 * d_t2 * I_3x3 + (d_th - sin_dth) / w_norm2 * sK + (1.0 / 2 * d_t2 - (1.0 - cos_dth) / w_norm2) * sK2; @@ -611,17 +591,17 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double d_t, const } else { // first order rotation integration with constant omega - Xi_1 = d_t * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); + Xi_1 = dt * (I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2); // second order rotation integration with constant omega - Xi_2 = 1.0 / 2 * d_t * Xi_1; + Xi_2 = 1.0 / 2 * dt * Xi_1; // first order integration with constant omega and constant acc Xi_3 = 1.0 / 2 * d_t2 * (sA + sin_dth * (-sA * sK + sK * sA + k_hat.dot(a_hat) * sK2) + (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + k_hat.dot(a_hat) * sK)); // second order integration with constant omega and constant acc - Xi_4 = 1.0 / 3 * d_t * Xi_3; + Xi_4 = 1.0 / 3 * dt * Xi_3; } // Store the integrated parameters @@ -685,7 +665,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Tg_id = local_size; local_size += state->_calib_imu_tg->size(); } - if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.imu_model == StateOptions::ImuModel::KALIBR) { th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); } else { @@ -831,7 +811,7 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d Tg_id = local_size; local_size += state->_calib_imu_tg->size(); } - if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.imu_model == StateOptions::ImuModel::KALIBR) { th_wtoI_id = local_size; local_size += state->_calib_imu_GYROtoIMU->size(); } else { @@ -944,7 +924,7 @@ Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eig assert(state->_options.do_calib_imu_intrinsics); Eigen::MatrixXd H_Dw = Eigen::MatrixXd::Zero(3, 6); - if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.imu_model == StateOptions::ImuModel::KALIBR) { H_Dw << w_1 * I_3x3, w_2 * e_2, w_2 * e_3, w_3 * e_3; } else { H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * I_3x3; @@ -964,7 +944,7 @@ Eigen::MatrixXd Propagator::compute_H_Da(std::shared_ptr state, const Eig assert(state->_options.do_calib_imu_intrinsics); Eigen::MatrixXd H_Da = Eigen::MatrixXd::Zero(3, 6); - if (state->_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (state->_options.imu_model == StateOptions::ImuModel::KALIBR) { H_Da << a_1 * I_3x3, a_2 * e_2, a_2 * e_3, a_3 * e_3; } else { H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * I_3x3; diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index ef513b709..8da21d2ba 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -37,14 +37,39 @@ namespace ov_msckf { * For derivations look at @ref propagation page which has detailed equations. */ class Propagator { - public: + /** + * @brief Struct of our imu noise parameters + */ + struct NoiseManager { + + /// Gyroscope white noise (rad/s/sqrt(hz)) + double sigma_w = 1.6968e-04; + + /// Gyroscope random walk (rad/s^2/sqrt(hz)) + double sigma_wb = 1.9393e-05; + + /// Accelerometer white noise (m/s^2/sqrt(hz)) + double sigma_a = 2.0000e-3; + + /// Accelerometer random walk (m/s^3/sqrt(hz)) + double sigma_ab = 3.0000e-03; + + /// Nice print function of what parameters we have loaded + void print() const { + PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); + } + }; + /** * @brief Default constructor - * @param imu_config imu noise characteristics (continuous time) + * @param noises imu noise characteristics (continuous time) * @param gravity_mag Global gravity magnitude of the system (normally 9.81) */ - Propagator(ov_core::ImuConfig imu_config, double gravity_mag) : _imu_config(imu_config) { + Propagator(NoiseManager noises, double gravity_mag) : _noises(noises) { last_prop_time_offset = 0.0; _gravity << 0.0, 0.0, gravity_mag; } @@ -178,17 +203,14 @@ class Propagator { * * @param state Pointer to state * @param dt Time we should integrate over - * @param w_hat1 Angular velocity with bias removed - * @param a_hat1 Linear acceleration with bias removed - * @param w_hat2 Next angular velocity with bias removed - * @param a_hat2 Next linear acceleration with bias removed + * @param w_hat Angular velocity with bias removed + * @param a_hat Linear acceleration with bias removed * @param new_q The resulting new orientation after integration * @param new_v The resulting new velocity after integration * @param new_p The resulting new position after integration */ - void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, - const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, - Eigen::Vector3d &new_p); + void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &w_hat, + Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p); /** * @brief RK4 imu mean propagation. @@ -425,7 +447,7 @@ class Propagator { Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); /// Container for the noise values - ov_core::ImuConfig _imu_config; + NoiseManager _noises; /// Our history of IMU messages (time, angular, linear) std::vector imu_data; diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index afffed921..8b732e820 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -42,23 +42,23 @@ State::State(StateOptions &options) { // NOTE: since if calibrating these will evolve / be correlated during propagation _calib_imu_dw = std::make_shared(6); _calib_imu_da = std::make_shared(6); - // if (options.imu_model == ImuConfig::ImuModel::KALIBR) { - // // lower triangular of the matrix (column-wise) - // Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); - // _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; - // _calib_imu_dw->set_value(_imu_default); - // _calib_imu_dw->set_fej(_imu_default); - // _calib_imu_da->set_value(_imu_default); - // _calib_imu_da->set_fej(_imu_default); - // } else { - // // upper triangular of the matrix (column-wise) - // Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); - // _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; - // _calib_imu_dw->set_value(_imu_default); - // _calib_imu_dw->set_fej(_imu_default); - // _calib_imu_da->set_value(_imu_default); - // _calib_imu_da->set_fej(_imu_default); - // } + if (options.imu_model == StateOptions::ImuModel::KALIBR) { + // lower triangular of the matrix (column-wise) + Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); + _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; + _calib_imu_dw->set_value(_imu_default); + _calib_imu_dw->set_fej(_imu_default); + _calib_imu_da->set_value(_imu_default); + _calib_imu_da->set_fej(_imu_default); + } else { + // upper triangular of the matrix (column-wise) + Eigen::Matrix _imu_default = Eigen::Matrix::Zero(); + _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0; + _calib_imu_dw->set_value(_imu_default); + _calib_imu_dw->set_fej(_imu_default); + _calib_imu_da->set_value(_imu_default); + _calib_imu_da->set_fej(_imu_default); + } _calib_imu_tg = std::make_shared(9); _calib_imu_GYROtoIMU = std::make_shared(); _calib_imu_ACCtoIMU = std::make_shared(); @@ -83,7 +83,7 @@ State::State(StateOptions &options) { // If kalibr model, R_GYROtoIMU is calibrated // If rpng model, R_ACCtoIMU is calibrated - if (options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (options.imu_model == StateOptions::ImuModel::KALIBR) { _calib_imu_GYROtoIMU->set_local_id(current_id); _variables.push_back(_calib_imu_GYROtoIMU); current_id += _calib_imu_GYROtoIMU->size(); @@ -140,7 +140,7 @@ State::State(StateOptions &options) { if (_options.do_calib_imu_g_sensitivity) { _Cov.block(_calib_imu_tg->id(), _calib_imu_tg->id(), 9, 9) = std::pow(0.005, 2) * Eigen::Matrix::Identity(); } - if (_options.imu_model == ImuConfig::ImuModel::KALIBR) { + if (_options.imu_model == StateOptions::ImuModel::KALIBR) { _Cov.block(_calib_imu_GYROtoIMU->id(), _calib_imu_GYROtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); } else { _Cov.block(_calib_imu_ACCtoIMU->id(), _calib_imu_ACCtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index c8d66f955..7740a9425 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -88,7 +88,7 @@ class State { */ Eigen::Matrix3d Dw() { Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if (_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { + if (_options.imu_model == StateOptions::ImuModel::KALIBR) { Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); } else { @@ -108,7 +108,7 @@ class State { */ Eigen::Matrix3d Da() { Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (_options.imu_model == ov_core::ImuConfig::ImuModel::KALIBR) { + if (_options.imu_model == StateOptions::ImuModel::KALIBR) { Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), _calib_imu_da->value()(4), _calib_imu_da->value()(5); } else { diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 8563b0705..f1157ac4b 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -37,14 +37,11 @@ struct StateOptions { /// Bool to determine whether or not to do first estimate Jacobians bool do_fej = true; - /// Bool to determine whether or not use imu message averaging - bool imu_avg = false; + /// Numerical integration methods + enum IntegrationMethod { DISCRETE, RK4, ANALYTICAL }; - /// Bool to determine if we should use Rk4 imu integration - bool use_rk4_integration = false; - - /// Bool to determine if we should use analytic imu integration - bool use_analytic_integration = true; + /// What type of numerical integration is used during propagation + IntegrationMethod integration_method = IntegrationMethod::RK4; /// Bool to determine whether or not to calibrate imu-to-camera pose bool do_calib_camera_pose = false; @@ -61,8 +58,11 @@ struct StateOptions { /// Bool to determine whether or not to calibrate the Gravity sensitivity bool do_calib_imu_g_sensitivity = false; - /// Indicator to use which model, 0: kalibr and 1: rpng - ov_core::ImuConfig::ImuModel imu_model = ov_core::ImuConfig::ImuModel::KALIBR; + /// IMU intrinsic models + enum ImuModel { KALIBR, RPNG }; + + /// What model our IMU intrinsics are + ImuModel imu_model = ImuModel::KALIBR; /// Max clone size of sliding window int max_clone_size = 11; @@ -95,9 +95,21 @@ struct StateOptions { void print(const std::shared_ptr &parser = nullptr) { if (parser != nullptr) { parser->parse_config("use_fej", do_fej); - parser->parse_config("use_imuavg", imu_avg); - parser->parse_config("use_rk4_int", use_rk4_integration); - parser->parse_config("use_analytic_int", use_analytic_integration); + + // Integration method + std::string integration_str = "rk4"; + parser->parse_config("integration", integration_str); + if (integration_str == "discrete") { + integration_method = IntegrationMethod::DISCRETE; + } else if (integration_str == "rk4") { + integration_method = IntegrationMethod::RK4; + } else if (integration_str == "analytical") { + integration_method = IntegrationMethod::ANALYTICAL; + } else { + PRINT_ERROR(RED "invalid imu integration model: %s\n" RESET, integration_str.c_str()); + PRINT_ERROR(RED "please select a valid model: discrete, rk4, analytical\n" RESET); + std::exit(EXIT_FAILURE); + } // Calibration booleans parser->parse_config("calib_cam_extrinsics", do_calib_camera_pose); @@ -129,11 +141,12 @@ struct StateOptions { std::string imu_model_str = "kalibr"; parser->parse_external("relative_config_imu", "imu0", "model", imu_model_str); if (imu_model_str == "kalibr" || imu_model_str == "calibrated") { - imu_model = ov_core::ImuConfig::ImuModel::KALIBR; + imu_model = ImuModel::KALIBR; } else if (imu_model_str == "rpng") { - imu_model = ov_core::ImuConfig::ImuModel::RPNG; + imu_model = ImuModel::RPNG; } else { PRINT_ERROR(RED "invalid imu model: %s\n" RESET, imu_model_str.c_str()); + PRINT_ERROR(RED "please select a valid model: kalibr, rpng\\n" RESET); std::exit(EXIT_FAILURE); } if (imu_model_str == "calibrated" && (do_calib_imu_intrinsics || do_calib_imu_g_sensitivity)) { @@ -143,9 +156,7 @@ struct StateOptions { } } PRINT_DEBUG(" - use_fej: %d\n", do_fej); - PRINT_DEBUG(" - use_imuavg: %d\n", imu_avg); - PRINT_DEBUG(" - use_rk4_int: %d\n", use_rk4_integration); - PRINT_DEBUG(" - use_analytic_int: %d\n", use_analytic_integration); + PRINT_DEBUG(" - integration: %d\n", integration_method); PRINT_DEBUG(" - calib_cam_extrinsics: %d\n", do_calib_camera_pose); PRINT_DEBUG(" - calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); PRINT_DEBUG(" - calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 5189368bd..5bf87b938 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -127,11 +127,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Measurement noise (convert from continuous to discrete) // Note the dt time might be different if we have "cut" any imu measurements - R.block(6 * i + 0, 6 * i + 0, 3, 3) *= std::pow(_imu_config.sigma_w, 2) / dt; + R.block(6 * i + 0, 6 * i + 0, 3, 3) *= std::pow(_noises.sigma_w, 2) / dt; if (!integrated_accel_constraint) { - R.block(6 * i + 3, 6 * i + 3, 3, 3) *= std::pow(_imu_config.sigma_a, 2) / dt; + R.block(6 * i + 3, 6 * i + 3, 3, 3) *= std::pow(_noises.sigma_a, 2) / dt; } else { - R.block(6 * i + 3, 6 * i + 3, 3, 3) *= std::pow(_imu_config.sigma_a, 2) * dt; + R.block(6 * i + 3, 6 * i + 3, 3, 3) *= std::pow(_noises.sigma_a, 2) * dt; } dt_summed += dt; } @@ -143,8 +143,8 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Next propagate the biases forward in time // NOTE: G*Qd*G^t = dt*Qd*dt = dt*Qc Eigen::MatrixXd Q_bias = Eigen::MatrixXd::Identity(6, 6); - Q_bias.block(0, 0, 3, 3) *= dt_summed * std::pow(_imu_config.sigma_wb, 2); - Q_bias.block(3, 3, 3, 3) *= dt_summed * std::pow(_imu_config.sigma_ab, 2); + Q_bias.block(0, 0, 3, 3) *= dt_summed * std::pow(_noises.sigma_wb, 2); + Q_bias.block(3, 3, 3, 3) *= dt_summed * std::pow(_noises.sigma_ab, 2); // Chi2 distance check // NOTE: we also append the propagation we "would do before the update" if this was to be accepted diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index c4e72b36f..6fe58fcd5 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -63,10 +63,10 @@ class UpdaterZeroVelocity { * @param zupt_noise_multiplier Multiplier of our IMU noise matrix (default should be 1.0) * @param zupt_max_disparity Max disparity we should consider to do a update with */ - UpdaterZeroVelocity(UpdaterOptions &options, ov_core::ImuConfig &imu_config, std::shared_ptr db, + UpdaterZeroVelocity(UpdaterOptions &options, Propagator::NoiseManager &noises, std::shared_ptr db, std::shared_ptr prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, double zupt_max_disparity) - : _options(options), _imu_config(imu_config), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), + : _options(options), _noises(noises), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), _zupt_noise_multiplier(zupt_noise_multiplier), _zupt_max_disparity(zupt_max_disparity) { // Gravity @@ -120,7 +120,7 @@ class UpdaterZeroVelocity { UpdaterOptions _options; /// Container for the imu noise values - ov_core::ImuConfig _imu_config; + Propagator::NoiseManager _noises; /// Feature tracker database with all features in it std::shared_ptr _db; From 1f733b50528aeed88b9e166e803fad4f3fa2f6bf Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 10 Jan 2022 16:35:06 -0500 Subject: [PATCH 32/60] simplify simulation plots, convert Da/Dw and Tg state functions to static, print final calibration results to console --- ov_eval/src/calc/ResultSimulation.cpp | 144 ++++++------ ov_eval/src/calc/ResultSimulation.h | 310 ++++++++++++++++++++++++++ ov_eval/src/error_comparison.cpp | 1 + ov_eval/src/error_simulation.cpp | 2 +- ov_eval/src/error_singlerun.cpp | 2 + ov_eval/src/plot_trajectories.cpp | 2 + ov_msckf/launch/simulation.launch | 2 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 36 +++ ov_msckf/src/ros/ROS2Visualizer.cpp | 35 +++ ov_msckf/src/sim/Simulator.cpp | 15 +- ov_msckf/src/state/Propagator.cpp | 64 +++--- ov_msckf/src/state/State.h | 49 ++-- 12 files changed, 507 insertions(+), 155 deletions(-) diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 5823a687f..ec7be8c2c 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -126,6 +126,7 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (deg)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== @@ -142,6 +143,7 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (m)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== @@ -158,6 +160,7 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (m/s)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== @@ -174,6 +177,7 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (rad/s)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== @@ -190,6 +194,7 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (m/s^2)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== @@ -232,7 +237,6 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) { return; #else - //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 250); @@ -267,8 +271,8 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) { matplotlibcpp::title("Camera IMU Time Offset Error"); matplotlibcpp::ylabel("error (sec)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); - //===================================================== #endif } @@ -336,7 +340,6 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; assert(error_cam_k.size() <= colors.size()); - //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 600); for (int n = 0; n < (int)est_state.at(0)(18); n++) { @@ -356,10 +359,9 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { matplotlibcpp::subplot(4, 1, 4); matplotlibcpp::ylabel("cy (px)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); - //===================================================== - //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 600); for (int n = 0; n < (int)est_state.at(0)(18); n++) { @@ -379,8 +381,8 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { matplotlibcpp::subplot(4, 1, 4); matplotlibcpp::ylabel("d4"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); - //===================================================== #endif } @@ -454,7 +456,6 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; assert(error_cam_ori.size() <= colors.size()); - //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 500); for (int n = 0; n < (int)est_state.at(0)(18); n++) { @@ -472,10 +473,9 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (deg)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); - //===================================================== - //===================================================== // Plot this figure matplotlibcpp::figure_size(800, 500); for (int n = 0; n < (int)est_state.at(0)(18); n++) { @@ -493,8 +493,8 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (m)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); - //===================================================== #endif } @@ -595,115 +595,95 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Plot line colors std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; - - //===================================================== - // Plot this figure - matplotlibcpp::figure_size(800, 500); std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(0); std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(1); - plot_3errors(error_dw[0], error_dw[1], error_dw[2], colors.at(0), stdcolor); + + // Plot this figure + matplotlibcpp::figure_size(1000, 500); + plot_6errors(error_dw[0], error_dw[3], error_dw[1], error_dw[4], error_dw[2], error_dw[5], colors.at(0), stdcolor); // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Dw Error"); + matplotlibcpp::subplot(3, 2, 1); + matplotlibcpp::title("IMU Dw Error (1:3)"); matplotlibcpp::ylabel("dw_1"); - matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::subplot(3, 2, 2); + matplotlibcpp::title("IMU Dw Error (4:6)"); + matplotlibcpp::ylabel("dw_4"); + matplotlibcpp::subplot(3, 2, 3); matplotlibcpp::ylabel("dw_2"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 2, 4); + matplotlibcpp::ylabel("dw_5"); + matplotlibcpp::subplot(3, 2, 5); matplotlibcpp::ylabel("dw_3"); matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_dw[3], error_dw[4], error_dw[5], colors.at(0), stdcolor); - - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Dw Error"); - matplotlibcpp::ylabel("dw_4"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("dw_5"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 2, 6); matplotlibcpp::ylabel("dw_6"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== //===================================================== // Plot this figure - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_da[0], error_da[1], error_da[2], colors.at(0), stdcolor); + matplotlibcpp::figure_size(1000, 500); + plot_6errors(error_da[0], error_da[3], error_da[1], error_da[4], error_da[2], error_da[5], colors.at(0), stdcolor); // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Da Error"); + matplotlibcpp::subplot(3, 2, 1); + matplotlibcpp::title("IMU Da Error (1:3)"); matplotlibcpp::ylabel("da_1"); - matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::subplot(3, 2, 2); + matplotlibcpp::title("IMU Da Error (4:6)"); + matplotlibcpp::ylabel("da_4"); + matplotlibcpp::subplot(3, 2, 3); matplotlibcpp::ylabel("da_2"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 2, 4); + matplotlibcpp::ylabel("da_5"); + matplotlibcpp::subplot(3, 2, 5); matplotlibcpp::ylabel("da_3"); matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_da[3], error_da[4], error_da[5], colors.at(0), stdcolor); - - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Da Error"); - matplotlibcpp::ylabel("da_4"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("da_5"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 2, 6); matplotlibcpp::ylabel("da_6"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== //===================================================== // Plot this figure - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_tg[0], error_tg[1], error_tg[2], colors.at(0), stdcolor); + // NOTE: display is row-based not column-based + matplotlibcpp::figure_size(1400, 500); + plot_9errors(error_tg[0], error_tg[3], error_tg[6], error_tg[1], error_tg[4], error_tg[7], error_tg[2], error_tg[5], error_tg[8], + colors.at(0), stdcolor); // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Tg Error"); + matplotlibcpp::subplot(3, 3, 1); + matplotlibcpp::title("IMU Tg Error (1:3)"); matplotlibcpp::ylabel("tg_1"); - matplotlibcpp::subplot(3, 1, 2); + matplotlibcpp::subplot(3, 3, 2); + matplotlibcpp::title("IMU Tg Error (4:6)"); + matplotlibcpp::ylabel("tg_4"); + matplotlibcpp::subplot(3, 3, 3); + matplotlibcpp::title("IMU Tg Error (7:9)"); + matplotlibcpp::ylabel("tg_7"); + matplotlibcpp::subplot(3, 3, 4); matplotlibcpp::ylabel("tg_2"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 3, 5); + matplotlibcpp::ylabel("tg_5"); + matplotlibcpp::subplot(3, 3, 6); + matplotlibcpp::ylabel("tg_8"); + matplotlibcpp::subplot(3, 3, 7); matplotlibcpp::ylabel("tg_3"); matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_tg[3], error_tg[4], error_tg[5], colors.at(0), stdcolor); - - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_4"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("tg_5"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 3, 8); matplotlibcpp::ylabel("tg_6"); matplotlibcpp::xlabel("dataset time (s)"); - matplotlibcpp::show(false); - - matplotlibcpp::figure_size(800, 500); - plot_3errors(error_tg[6], error_tg[7], error_tg[8], colors.at(0), stdcolor); - - // Update the title and axis labels - matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU Tg Error"); - matplotlibcpp::ylabel("tg_7"); - matplotlibcpp::subplot(3, 1, 2); - matplotlibcpp::ylabel("tg_8"); - matplotlibcpp::subplot(3, 1, 3); + matplotlibcpp::subplot(3, 3, 9); matplotlibcpp::ylabel("tg_9"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); //===================================================== @@ -719,13 +699,14 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU R_GyrotoI Error"); + matplotlibcpp::title("IMU R_GYROtoIMU Error"); matplotlibcpp::ylabel("x-error (deg)"); matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("y-error (deg)"); matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (deg)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); } else { @@ -737,13 +718,14 @@ void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) { // Update the title and axis labels matplotlibcpp::subplot(3, 1, 1); - matplotlibcpp::title("IMU R_AcctoI Error"); + matplotlibcpp::title("IMU R_ACCtoIMU Error"); matplotlibcpp::ylabel("x-error (deg)"); matplotlibcpp::subplot(3, 1, 2); matplotlibcpp::ylabel("y-error (deg)"); matplotlibcpp::subplot(3, 1, 3); matplotlibcpp::ylabel("z-error (deg)"); matplotlibcpp::xlabel("dataset time (s)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); } diff --git a/ov_eval/src/calc/ResultSimulation.h b/ov_eval/src/calc/ResultSimulation.h index 3ae736282..8acb5416f 100644 --- a/ov_eval/src/calc/ResultSimulation.h +++ b/ov_eval/src/calc/ResultSimulation.h @@ -273,6 +273,316 @@ class ResultSimulation { } matplotlibcpp::xlim(0.0, endtime4 - starttime4); } + /** + * @brief Plots six different statistic values and sigma bounds + * @param s1 Error one + * @param s2 Error two + * @param s3 Error three + * @param s4 Error four + * @param s5 Error five + * @param s6 Error six + * @param color_err MATLAB color string for error line (blue, red, etc.) + * @param color_std MATLAB color string for deviation (blue, red, etc.) + */ + void plot_6errors(ov_eval::Statistics s1, ov_eval::Statistics s2, ov_eval::Statistics s3, ov_eval::Statistics s4, ov_eval::Statistics s5, + ov_eval::Statistics s6, std::string color_err, std::string color_std) { + + // Zero our time arrays + double starttime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(0); + double endtime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(s1.timestamps.size() - 1); + for (size_t i = 0; i < s1.timestamps.size(); i++) { + s1.timestamps.at(i) -= starttime1; + } + double starttime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(0); + double endtime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(s2.timestamps.size() - 1); + for (size_t i = 0; i < s2.timestamps.size(); i++) { + s2.timestamps.at(i) -= starttime2; + } + double starttime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(0); + double endtime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(s3.timestamps.size() - 1); + for (size_t i = 0; i < s3.timestamps.size(); i++) { + s3.timestamps.at(i) -= starttime3; + } + double starttime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(0); + double endtime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(s4.timestamps.size() - 1); + for (size_t i = 0; i < s4.timestamps.size(); i++) { + s4.timestamps.at(i) -= starttime4; + } + double starttime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(0); + double endtime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(s5.timestamps.size() - 1); + for (size_t i = 0; i < s5.timestamps.size(); i++) { + s5.timestamps.at(i) -= starttime5; + } + double starttime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(0); + double endtime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(s6.timestamps.size() - 1); + for (size_t i = 0; i < s6.timestamps.size(); i++) { + s6.timestamps.at(i) -= starttime6; + } + + // Parameters that define the line styles + std::map params_value, params_bound; + // params_value.insert({"label","error"}); + params_value.insert({"linestyle", "-"}); + params_value.insert({"color", color_err}); + // params_bound.insert({"label","3 sigma bound"}); + params_bound.insert({"linestyle", "--"}); + params_bound.insert({"color", color_std}); + + // Plot our error value + matplotlibcpp::subplot(3, 2, 1); + matplotlibcpp::plot(s1.timestamps, s1.values, params_value); + if (!s1.values_bound.empty()) { + matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound); + for (size_t i = 0; i < s1.timestamps.size(); i++) { + s1.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime1 - starttime1); + + // Plot our error value + matplotlibcpp::subplot(3, 2, 2); + matplotlibcpp::plot(s2.timestamps, s2.values, params_value); + if (!s2.values_bound.empty()) { + matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound); + for (size_t i = 0; i < s2.timestamps.size(); i++) { + s2.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime2 - starttime2); + + // Plot our error value + matplotlibcpp::subplot(3, 2, 3); + matplotlibcpp::plot(s3.timestamps, s3.values, params_value); + if (!s3.values_bound.empty()) { + matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound); + for (size_t i = 0; i < s3.timestamps.size(); i++) { + s3.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime3 - starttime3); + + // Plot our error value + matplotlibcpp::subplot(3, 2, 4); + matplotlibcpp::plot(s4.timestamps, s4.values, params_value); + if (!s4.values_bound.empty()) { + matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound); + for (size_t i = 0; i < s4.timestamps.size(); i++) { + s4.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime4 - starttime4); + + // Plot our error value + matplotlibcpp::subplot(3, 2, 5); + matplotlibcpp::plot(s5.timestamps, s5.values, params_value); + if (!s5.values_bound.empty()) { + matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound); + for (size_t i = 0; i < s5.timestamps.size(); i++) { + s5.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime5 - starttime5); + + // Plot our error value + matplotlibcpp::subplot(3, 2, 6); + matplotlibcpp::plot(s6.timestamps, s6.values, params_value); + if (!s6.values_bound.empty()) { + matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound); + for (size_t i = 0; i < s6.timestamps.size(); i++) { + s6.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime6 - starttime6); + } + + /** + * @brief Plots six different statistic values and sigma bounds + * @param s1 Error one + * @param s2 Error two + * @param s3 Error three + * @param s4 Error four + * @param s5 Error five + * @param s6 Error six + * @param s7 Error four + * @param s8 Error five + * @param s9 Error six + * @param color_err MATLAB color string for error line (blue, red, etc.) + * @param color_std MATLAB color string for deviation (blue, red, etc.) + */ + void plot_9errors(ov_eval::Statistics s1, ov_eval::Statistics s2, ov_eval::Statistics s3, ov_eval::Statistics s4, ov_eval::Statistics s5, + ov_eval::Statistics s6, ov_eval::Statistics s7, ov_eval::Statistics s8, ov_eval::Statistics s9, std::string color_err, + std::string color_std) { + + // Zero our time arrays + double starttime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(0); + double endtime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(s1.timestamps.size() - 1); + for (size_t i = 0; i < s1.timestamps.size(); i++) { + s1.timestamps.at(i) -= starttime1; + } + double starttime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(0); + double endtime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(s2.timestamps.size() - 1); + for (size_t i = 0; i < s2.timestamps.size(); i++) { + s2.timestamps.at(i) -= starttime2; + } + double starttime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(0); + double endtime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(s3.timestamps.size() - 1); + for (size_t i = 0; i < s3.timestamps.size(); i++) { + s3.timestamps.at(i) -= starttime3; + } + double starttime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(0); + double endtime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(s4.timestamps.size() - 1); + for (size_t i = 0; i < s4.timestamps.size(); i++) { + s4.timestamps.at(i) -= starttime4; + } + double starttime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(0); + double endtime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(s5.timestamps.size() - 1); + for (size_t i = 0; i < s5.timestamps.size(); i++) { + s5.timestamps.at(i) -= starttime5; + } + double starttime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(0); + double endtime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(s6.timestamps.size() - 1); + for (size_t i = 0; i < s6.timestamps.size(); i++) { + s6.timestamps.at(i) -= starttime6; + } + double starttime7 = (s7.timestamps.empty()) ? 0 : s7.timestamps.at(0); + double endtime7 = (s7.timestamps.empty()) ? 0 : s7.timestamps.at(s7.timestamps.size() - 1); + for (size_t i = 0; i < s7.timestamps.size(); i++) { + s7.timestamps.at(i) -= starttime7; + } + double starttime8 = (s8.timestamps.empty()) ? 0 : s8.timestamps.at(0); + double endtime8 = (s8.timestamps.empty()) ? 0 : s8.timestamps.at(s8.timestamps.size() - 1); + for (size_t i = 0; i < s8.timestamps.size(); i++) { + s8.timestamps.at(i) -= starttime8; + } + double starttime9 = (s9.timestamps.empty()) ? 0 : s9.timestamps.at(0); + double endtime9 = (s9.timestamps.empty()) ? 0 : s9.timestamps.at(s9.timestamps.size() - 1); + for (size_t i = 0; i < s9.timestamps.size(); i++) { + s9.timestamps.at(i) -= starttime9; + } + + // Parameters that define the line styles + std::map params_value, params_bound; + // params_value.insert({"label","error"}); + params_value.insert({"linestyle", "-"}); + params_value.insert({"color", color_err}); + // params_bound.insert({"label","3 sigma bound"}); + params_bound.insert({"linestyle", "--"}); + params_bound.insert({"color", color_std}); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 1); + matplotlibcpp::plot(s1.timestamps, s1.values, params_value); + if (!s1.values_bound.empty()) { + matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound); + for (size_t i = 0; i < s1.timestamps.size(); i++) { + s1.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime1 - starttime1); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 2); + matplotlibcpp::plot(s2.timestamps, s2.values, params_value); + if (!s2.values_bound.empty()) { + matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound); + for (size_t i = 0; i < s2.timestamps.size(); i++) { + s2.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime2 - starttime2); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 3); + matplotlibcpp::plot(s3.timestamps, s3.values, params_value); + if (!s3.values_bound.empty()) { + matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound); + for (size_t i = 0; i < s3.timestamps.size(); i++) { + s3.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime3 - starttime3); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 4); + matplotlibcpp::plot(s4.timestamps, s4.values, params_value); + if (!s4.values_bound.empty()) { + matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound); + for (size_t i = 0; i < s4.timestamps.size(); i++) { + s4.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime4 - starttime4); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 5); + matplotlibcpp::plot(s5.timestamps, s5.values, params_value); + if (!s5.values_bound.empty()) { + matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound); + for (size_t i = 0; i < s5.timestamps.size(); i++) { + s5.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime5 - starttime5); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 6); + matplotlibcpp::plot(s6.timestamps, s6.values, params_value); + if (!s6.values_bound.empty()) { + matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound); + for (size_t i = 0; i < s6.timestamps.size(); i++) { + s6.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime6 - starttime6); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 7); + matplotlibcpp::plot(s7.timestamps, s7.values, params_value); + if (!s7.values_bound.empty()) { + matplotlibcpp::plot(s7.timestamps, s7.values_bound, params_bound); + for (size_t i = 0; i < s7.timestamps.size(); i++) { + s7.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s7.timestamps, s7.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime7 - starttime7); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 8); + matplotlibcpp::plot(s8.timestamps, s8.values, params_value); + if (!s8.values_bound.empty()) { + matplotlibcpp::plot(s8.timestamps, s8.values_bound, params_bound); + for (size_t i = 0; i < s8.timestamps.size(); i++) { + s8.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s8.timestamps, s8.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime8 - starttime8); + + // Plot our error value + matplotlibcpp::subplot(3, 3, 9); + matplotlibcpp::plot(s9.timestamps, s9.values, params_value); + if (!s9.values_bound.empty()) { + matplotlibcpp::plot(s9.timestamps, s9.values_bound, params_bound); + for (size_t i = 0; i < s9.timestamps.size(); i++) { + s9.values_bound.at(i) *= -1; + } + matplotlibcpp::plot(s9.timestamps, s9.values_bound, params_bound); + } + matplotlibcpp::xlim(0.0, endtime9 - starttime9); + } #endif }; diff --git a/ov_eval/src/error_comparison.cpp b/ov_eval/src/error_comparison.cpp index 4626dfb90..6cf110283 100644 --- a/ov_eval/src/error_comparison.cpp +++ b/ov_eval/src/error_comparison.cpp @@ -382,6 +382,7 @@ int main(int argc, char **argv) { matplotlibcpp::title("Relative Position Error"); matplotlibcpp::ylabel("translational error (m)"); matplotlibcpp::xlabel("sub-segment lengths (m)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(true); #endif diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp index 663a482aa..57a1b147f 100644 --- a/ov_eval/src/error_simulation.cpp +++ b/ov_eval/src/error_simulation.cpp @@ -65,7 +65,7 @@ int main(int argc, char **argv) { // Plot IMU intrinsics PRINT_INFO("Plotting IMU intrinsics...\n"); - traj.plot_imu_intrinsics(true); + traj.plot_imu_intrinsics(true, 60); #ifdef HAVE_PYTHONLIBS matplotlibcpp::show(true); diff --git a/ov_eval/src/error_singlerun.cpp b/ov_eval/src/error_singlerun.cpp index a587a43c3..07d9f0d80 100644 --- a/ov_eval/src/error_singlerun.cpp +++ b/ov_eval/src/error_singlerun.cpp @@ -172,6 +172,7 @@ int main(int argc, char **argv) { matplotlibcpp::title("Relative Orientation Error"); matplotlibcpp::ylabel("orientation error (deg)"); matplotlibcpp::xlabel("sub-segment lengths (m)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); // Plot this figure @@ -189,6 +190,7 @@ int main(int argc, char **argv) { matplotlibcpp::title("Relative Position Error"); matplotlibcpp::ylabel("translation error (m)"); matplotlibcpp::xlabel("sub-segment lengths (m)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); #endif diff --git a/ov_eval/src/plot_trajectories.cpp b/ov_eval/src/plot_trajectories.cpp index 7e889ca51..24e3ffe64 100644 --- a/ov_eval/src/plot_trajectories.cpp +++ b/ov_eval/src/plot_trajectories.cpp @@ -177,6 +177,7 @@ int main(int argc, char **argv) { // Display to the user matplotlibcpp::xlabel("x-axis (m)"); matplotlibcpp::ylabel("y-axis (m)"); + matplotlibcpp::tight_layout(); matplotlibcpp::show(false); // Plot this figure @@ -201,6 +202,7 @@ int main(int argc, char **argv) { matplotlibcpp::ylabel("z-axis (m)"); matplotlibcpp::xlim(0.0, endtime - starttime); matplotlibcpp::legend(); + matplotlibcpp::tight_layout(); matplotlibcpp::show(true); #endif diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 8c1f166bd..665724195 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -10,7 +10,7 @@ - + diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index e991e3e0c..fae0b384b 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -319,6 +319,42 @@ void ROS1Visualizer::visualize_final() { } } + // IMU intrinsics + if (_app->get_state()->_options.do_calib_imu_intrinsics) { + Eigen::Matrix3d Dw = State::Dm(_app->get_state()->_options.imu_model, _app->get_state()->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(_app->get_state()->_options.imu_model, _app->get_state()->_calib_imu_da->value()); + Eigen::Matrix3d Tw = Dw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d Ta = Da.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d R_IMUtoACC = _app->get_state()->_calib_imu_ACCtoIMU->Rot().transpose(); + Eigen::Matrix3d R_IMUtoGYRO = _app->get_state()->_calib_imu_GYROtoIMU->Rot().transpose(); + PRINT_INFO(REDPURPLE "Tw:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Tw(0, 0), Tw(0, 1), Tw(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Tw(1, 0), Tw(1, 1), Tw(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, Tw(2, 0), Tw(2, 1), Tw(2, 2)); + PRINT_INFO(REDPURPLE "Ta:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Ta(0, 0), Ta(0, 1), Ta(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Ta(1, 0), Ta(1, 1), Ta(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, Ta(2, 0), Ta(2, 1), Ta(2, 2)); + PRINT_INFO(REDPURPLE "R_IMUtoACC:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoACC(0, 0), R_IMUtoACC(0, 1), R_IMUtoACC(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoACC(1, 0), R_IMUtoACC(1, 1), R_IMUtoACC(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, R_IMUtoACC(2, 0), R_IMUtoACC(2, 1), R_IMUtoACC(2, 2)); + PRINT_INFO(REDPURPLE "R_IMUtoGYRO:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoGYRO(0, 0), R_IMUtoGYRO(0, 1), R_IMUtoGYRO(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoGYRO(1, 0), R_IMUtoGYRO(1, 1), R_IMUtoGYRO(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, R_IMUtoGYRO(2, 0), R_IMUtoGYRO(2, 1), R_IMUtoGYRO(2, 2)); + } + + // IMU intrinsics gravity sensitivity + if (_app->get_state()->_options.do_calib_imu_g_sensitivity) { + Eigen::Matrix3d Tg = State::Tg(_app->get_state()->_calib_imu_tg->value()); + PRINT_INFO(REDPURPLE "Tg:\n" RESET); + PRINT_INFO(REDPURPLE "%.6f,%.6f,%.6f,\n" RESET, Tg(0, 0), Tg(0, 1), Tg(0, 2)); + PRINT_INFO(REDPURPLE "%.6f,%.6f,%.6f,\n" RESET, Tg(1, 0), Tg(1, 1), Tg(1, 2)); + PRINT_INFO(REDPURPLE "%.6f,%.6f,%.6f\n\n" RESET, Tg(2, 0), Tg(2, 1), Tg(2, 2)); + } + + // Publish RMSE if we have it if (!gt_states.empty()) { PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 51a5f91d2..18ca1106a 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -330,6 +330,41 @@ void ROS2Visualizer::visualize_final() { } } + // IMU intrinsics + if (_app->get_state()->_options.do_calib_imu_intrinsics) { + Eigen::Matrix3d Dw = State::Dm(_app->get_state()->_options.imu_model, _app->get_state()->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(_app->get_state()->_options.imu_model, _app->get_state()->_calib_imu_da->value()); + Eigen::Matrix3d Tw = Dw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d Ta = Da.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); + Eigen::Matrix3d R_IMUtoACC = _app->get_state()->_calib_imu_ACCtoIMU->Rot().transpose(); + Eigen::Matrix3d R_IMUtoGYRO = _app->get_state()->_calib_imu_GYROtoIMU->Rot().transpose(); + PRINT_INFO(REDPURPLE "Tw:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Tw(0, 0), Tw(0, 1), Tw(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Tw(1, 0), Tw(1, 1), Tw(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, Tw(2, 0), Tw(2, 1), Tw(2, 2)); + PRINT_INFO(REDPURPLE "Ta:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Ta(0, 0), Ta(0, 1), Ta(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, Ta(1, 0), Ta(1, 1), Ta(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, Ta(2, 0), Ta(2, 1), Ta(2, 2)); + PRINT_INFO(REDPURPLE "R_IMUtoACC:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoACC(0, 0), R_IMUtoACC(0, 1), R_IMUtoACC(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoACC(1, 0), R_IMUtoACC(1, 1), R_IMUtoACC(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, R_IMUtoACC(2, 0), R_IMUtoACC(2, 1), R_IMUtoACC(2, 2)); + PRINT_INFO(REDPURPLE "R_IMUtoGYRO:\n" RESET); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoGYRO(0, 0), R_IMUtoGYRO(0, 1), R_IMUtoGYRO(0, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,\n" RESET, R_IMUtoGYRO(1, 0), R_IMUtoGYRO(1, 1), R_IMUtoGYRO(1, 2)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f\n\n" RESET, R_IMUtoGYRO(2, 0), R_IMUtoGYRO(2, 1), R_IMUtoGYRO(2, 2)); + } + + // IMU intrinsics gravity sensitivity + if (_app->get_state()->_options.do_calib_imu_g_sensitivity) { + Eigen::Matrix3d Tg = State::Tg(_app->get_state()->_calib_imu_tg->value()); + PRINT_INFO(REDPURPLE "Tg:\n" RESET); + PRINT_INFO(REDPURPLE "%.6f,%.6f,%.6f,\n" RESET, Tg(0, 0), Tg(0, 1), Tg(0, 2)); + PRINT_INFO(REDPURPLE "%.6f,%.6f,%.6f,\n" RESET, Tg(1, 0), Tg(1, 1), Tg(1, 2)); + PRINT_INFO(REDPURPLE "%.6f,%.6f,%.6f\n\n" RESET, Tg(2, 0), Tg(2, 1), Tg(2, 2)); + } + // Publish RMSE if we have it if (!gt_states.empty()) { PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index b0a297522..9236a0b9e 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -333,18 +333,9 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto // Get our imu intrinsic parameters // - kalibr: lower triangular of the matrix is used // - rpng: upper triangular of the matrix is used - Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (params.state_options.imu_model == StateOptions::ImuModel::KALIBR) { - Dw << params.vec_dw(0), 0, 0, params.vec_dw(1), params.vec_dw(3), 0, params.vec_dw(2), params.vec_dw(4), params.vec_dw(5); - Da << params.vec_da(0), 0, 0, params.vec_da(1), params.vec_da(3), 0, params.vec_da(2), params.vec_da(4), params.vec_da(5); - } else { - Dw << params.vec_dw(0), params.vec_dw(1), params.vec_dw(3), 0, params.vec_dw(2), params.vec_dw(4), 0, 0, params.vec_dw(5); - Da << params.vec_da(0), params.vec_da(1), params.vec_da(3), 0, params.vec_da(2), params.vec_da(4), 0, 0, params.vec_da(5); - } - Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << params.vec_tg(0), params.vec_tg(3), params.vec_tg(6), params.vec_tg(1), params.vec_tg(4), params.vec_tg(7), params.vec_tg(2), - params.vec_tg(5), params.vec_tg(8); + Eigen::Matrix3d Dw = State::Dm(params.state_options.imu_model, params.vec_dw); + Eigen::Matrix3d Da = State::Dm(params.state_options.imu_model, params.vec_da); + Eigen::Matrix3d Tg = State::Tg(params.vec_tg); // Get the readings with the imu intrinsic "distortion" Eigen::Matrix3d Tw = Dw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity()); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 6cbb63d38..167f5bd32 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -95,9 +95,11 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest Eigen::Vector3d last_a = Eigen::Vector3d::Zero(); Eigen::Vector3d last_w = Eigen::Vector3d::Zero(); if (!prop_data.empty()) { - last_a = state->_calib_imu_ACCtoIMU->Rot() * state->Da() * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); - last_w = state->_calib_imu_GYROtoIMU->Rot() * state->Dw() * - (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - state->Tg() * last_a); + Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); + Eigen::Matrix3d Tg = State::Tg(state->_calib_imu_tg->value()); + last_a = state->_calib_imu_ACCtoIMU->Rot() * Da * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); + last_w = state->_calib_imu_GYROtoIMU->Rot() * Dw * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - Tg * last_a); } // Do the update to the covariance with our "summed" state transition and IMU noise addition... @@ -144,6 +146,11 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // Save the original IMU state Eigen::VectorXd orig_val = state->_imu->value(); Eigen::VectorXd orig_fej = state->_imu->fej(); + + // IMU intrinsic calibration estimates (static) + Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); + Eigen::Matrix3d Tg = State::Tg(state->_calib_imu_tg->value()); // Loop through all IMU messages, and use them to move the state forward in time // This uses the zero'th order quat, and then constant acceleration discrete @@ -163,20 +170,20 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // Correct imu readings with IMU intrinsics Eigen::Matrix3d R_ACCtoIMU = state->_calib_imu_ACCtoIMU->Rot(); - a_hat1 = R_ACCtoIMU * state->Da() * a_hat1; - a_hat2 = R_ACCtoIMU * state->Da() * a_hat2; - a_hat_avg = R_ACCtoIMU * state->Da() * a_hat_avg; + a_hat1 = R_ACCtoIMU * Da * a_hat1; + a_hat2 = R_ACCtoIMU * Da * a_hat2; + a_hat_avg = R_ACCtoIMU * Da * a_hat_avg; // Corrected imu gyro measurements with our current biases - Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - state->Tg() * a_hat1; - Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - state->Tg() * a_hat2; + Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - Tg * a_hat1; + Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - Tg * a_hat2; Eigen::Vector3d w_hat_avg = .5 * (w_hat1 + w_hat2); // Correct imu readings with IMU intrinsics Eigen::Matrix3d R_GYROtoIMU = state->_calib_imu_GYROtoIMU->Rot(); - w_hat1 = R_GYROtoIMU * state->Dw() * w_hat1; - w_hat2 = R_GYROtoIMU * state->Dw() * w_hat2; - w_hat_avg = R_GYROtoIMU * state->Dw() * w_hat_avg; + w_hat1 = R_GYROtoIMU * Dw * w_hat1; + w_hat2 = R_GYROtoIMU * Dw * w_hat2; + w_hat_avg = R_GYROtoIMU * Dw * w_hat_avg; // Pre-compute some analytical values Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); @@ -345,6 +352,11 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Time elapsed over interval double dt = data_plus.timestamp - data_minus.timestamp; // assert(data_plus.timestamp>data_minus.timestamp); + + // IMU intrinsic calibration estimates (static) + Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); + Eigen::Matrix3d Tg = State::Tg(state->_calib_imu_tg->value()); // Corrected imu acc measurements with our current biases Eigen::Vector3d a_hat1 = data_minus.am - state->_imu->bias_a(); @@ -354,21 +366,21 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core // Convert "raw" imu to its corrected frame using the IMU intrinsics Eigen::Vector3d a_uncorrected = a_hat_avg; Eigen::Matrix3d R_ACCtoIMU = state->_calib_imu_ACCtoIMU->Rot(); - a_hat1 = R_ACCtoIMU * state->Da() * a_hat1; - a_hat2 = R_ACCtoIMU * state->Da() * a_hat2; - a_hat_avg = R_ACCtoIMU * state->Da() * a_hat_avg; + a_hat1 = R_ACCtoIMU * Da * a_hat1; + a_hat2 = R_ACCtoIMU * Da * a_hat2; + a_hat_avg = R_ACCtoIMU * Da * a_hat_avg; // Corrected imu gyro measurements with our current biases and gravity sensitivity - Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - state->Tg() * a_hat1; - Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - state->Tg() * a_hat2; + Eigen::Vector3d w_hat1 = data_minus.wm - state->_imu->bias_g() - Tg * a_hat1; + Eigen::Vector3d w_hat2 = data_plus.wm - state->_imu->bias_g() - Tg * a_hat2; Eigen::Vector3d w_hat_avg = .5 * (w_hat1 + w_hat2); // Convert "raw" imu to its corrected frame using the IMU intrinsics Eigen::Vector3d w_uncorrected = w_hat_avg; Eigen::Matrix3d R_GYROtoIMU = state->_calib_imu_GYROtoIMU->Rot(); - w_hat1 = R_GYROtoIMU * state->Dw() * w_hat1; - w_hat2 = R_GYROtoIMU * state->Dw() * w_hat2; - w_hat_avg = R_GYROtoIMU * state->Dw() * w_hat_avg; + w_hat1 = R_GYROtoIMU * Dw * w_hat1; + w_hat2 = R_GYROtoIMU * Dw * w_hat2; + w_hat_avg = R_GYROtoIMU * Dw * w_hat_avg; // Pre-compute some analytical values for the mean and covariance integration Eigen::Matrix Xi_sum = Eigen::Matrix::Zero(3, 18); @@ -398,8 +410,6 @@ void Propagator::predict_and_compute(std::shared_ptr state, const ov_core compute_F_and_G_discrete(state, dt, w_hat_avg, a_hat_avg, w_uncorrected, a_uncorrected, new_q, new_v, new_p, F, G); } - - // Construct our discrete noise covariance matrix // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report @@ -688,9 +698,9 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d } Eigen::Matrix3d dR_ktok1 = quat_2_Rot(new_q) * R_k.transpose(); - Eigen::Matrix3d Da = state->Da(); - Eigen::Matrix3d Dw = state->Dw(); - Eigen::Matrix3d Tg = state->Tg(); + Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); + Eigen::Matrix3d Tg = State::Tg(state->_calib_imu_tg->value()); Eigen::Matrix3d R_atoI = state->_calib_imu_ACCtoIMU->Rot(); Eigen::Matrix3d R_wtoI = state->_calib_imu_GYROtoIMU->Rot(); Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; @@ -836,9 +846,9 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // This is the change in the orientation from the end of the last prop to the current prop // This is needed since we need to include the "k-th" updated orientation information - Eigen::Matrix3d Da = state->Da(); - Eigen::Matrix3d Dw = state->Dw(); - Eigen::Matrix3d Tg = state->Tg(); + Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); + Eigen::Matrix3d Tg = State::Tg(state->_calib_imu_tg->value()); Eigen::Matrix3d R_atoI = state->_calib_imu_ACCtoIMU->Rot(); Eigen::Matrix3d R_wtoI = state->_calib_imu_GYROtoIMU->Rot(); Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 7740a9425..9f906e13a 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -79,54 +79,37 @@ class State { int max_covariance_size() { return (int)_Cov.rows(); } /** - * @brief Gyroscope intrinsic (scale imperfection and axis misalignment) + * @brief Gyroscope and accelerometer intrinsic matrix (scale imperfection and axis misalignment) * * If kalibr model, lower triangular of the matrix is used * If rpng model, upper triangular of the matrix is used * - * @return 3x3 matrix of current imu gyroscope intrinsics + * @return 3x3 matrix of current imu gyroscope / accelerometer intrinsics */ - Eigen::Matrix3d Dw() { - Eigen::Matrix3d Dw = Eigen::Matrix3d::Identity(); - if (_options.imu_model == StateOptions::ImuModel::KALIBR) { - Dw << _calib_imu_dw->value()(0), 0, 0, _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), - _calib_imu_dw->value()(4), _calib_imu_dw->value()(5); + static Eigen::Matrix3d Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd &vec) { + assert(vec.rows() == 6); + assert(vec.cols() == 1); + Eigen::Matrix3d D_matrix = Eigen::Matrix3d::Identity(); + if (imu_model == StateOptions::ImuModel::KALIBR) { + D_matrix << vec(0), 0, 0, vec(1), vec(3), 0, vec(2), vec(4), vec(5); } else { - Dw << _calib_imu_dw->value()(0), _calib_imu_dw->value()(1), _calib_imu_dw->value()(3), 0, _calib_imu_dw->value()(2), - _calib_imu_dw->value()(4), 0, 0, _calib_imu_dw->value()(5); + D_matrix << vec(0), vec(1), vec(3), 0, vec(2), vec(4), 0, 0, vec(5); } - return Dw; + return D_matrix; } /** - * @brief Accelerometer intrinsic (scale imperfection and axis misalignment) + * @brief Gyroscope gravity sensitivity * - * If kalibr model, lower triangular of the matrix is used - * If rpng model, upper triangular of the matrix is used + * For both kalibr and rpng models, this a 3x3 that is column-wise filled. * - * @return 3x3 matrix of current imu accelerometer intrinsics - */ - Eigen::Matrix3d Da() { - Eigen::Matrix3d Da = Eigen::Matrix3d::Identity(); - if (_options.imu_model == StateOptions::ImuModel::KALIBR) { - Da << _calib_imu_da->value()(0), 0, 0, _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), - _calib_imu_da->value()(4), _calib_imu_da->value()(5); - } else { - Da << _calib_imu_da->value()(0), _calib_imu_da->value()(1), _calib_imu_da->value()(3), 0, _calib_imu_da->value()(2), - _calib_imu_da->value()(4), 0, 0, _calib_imu_da->value()(5); - } - return Da; - } - - /** - * @brief Gyroscope gravity sensitivity * @return 3x3 matrix of current gravity sensitivity */ - Eigen::Matrix3d Tg() { + static Eigen::Matrix3d Tg(const Eigen::MatrixXd &vec) { + assert(vec.rows() == 9); + assert(vec.cols() == 1); Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero(); - Tg << _calib_imu_tg->value()(0), _calib_imu_tg->value()(3), _calib_imu_tg->value()(6), _calib_imu_tg->value()(1), - _calib_imu_tg->value()(4), _calib_imu_tg->value()(7), _calib_imu_tg->value()(2), _calib_imu_tg->value()(5), - _calib_imu_tg->value()(8); + Tg << vec(0), vec(3), vec(6), vec(1), vec(4), vec(7), vec(2), vec(5), vec(8); return Tg; } From 57ad3b18d8b9da019a05df6ca475b627b27a136e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 10 Jan 2022 16:42:17 -0500 Subject: [PATCH 33/60] reorder propagation class, make intrinic jacobs static --- ov_msckf/src/state/Propagator.cpp | 238 +++++++++++++++--------------- ov_msckf/src/state/Propagator.h | 131 ++++++++-------- 2 files changed, 185 insertions(+), 184 deletions(-) diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 167f5bd32..c9909d745 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -146,7 +146,7 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times // Save the original IMU state Eigen::VectorXd orig_val = state->_imu->value(); Eigen::VectorXd orig_fej = state->_imu->fej(); - + // IMU intrinsic calibration estimates (static) Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); @@ -229,130 +229,13 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times state->_imu->set_fej(orig_fej); } -std::vector Propagator::select_imu_readings(const std::vector &imu_data, double time0, double time1, - bool warn) { - - // Our vector imu readings - std::vector prop_data; - - // Ensure we have some measurements in the first place! - if (imu_data.empty()) { - if (warn) - PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): No IMU measurements. IMU-CAMERA are likely messed up!!!\n" RESET); - return prop_data; - } - - // Loop through and find all the needed measurements to propagate with - // Note we split measurements based on the given state time, and the update timestamp - for (size_t i = 0; i < imu_data.size() - 1; i++) { - - // START OF THE INTEGRATION PERIOD - // If the next timestamp is greater then our current state time - // And the current is not greater then it yet... - // Then we should "split" our current IMU measurement - if (imu_data.at(i + 1).timestamp > time0 && imu_data.at(i).timestamp < time0) { - ov_core::ImuData data = Propagator::interpolate_data(imu_data.at(i), imu_data.at(i + 1), time0); - prop_data.push_back(data); - // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n", - // (int)i,data.timestamp-prop_data.at(0).timestamp,time0-prop_data.at(0).timestamp); - continue; - } - - // MIDDLE OF INTEGRATION PERIOD - // If our imu measurement is right in the middle of our propagation period - // Then we should just append the whole measurement time to our propagation vector - if (imu_data.at(i).timestamp >= time0 && imu_data.at(i + 1).timestamp <= time1) { - prop_data.push_back(imu_data.at(i)); - // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp); - continue; - } - - // END OF THE INTEGRATION PERIOD - // If the current timestamp is greater then our update time - // We should just "split" the NEXT IMU measurement to the update time, - // NOTE: we add the current time, and then the time at the end of the interval (so we can get a dt) - // NOTE: we also break out of this loop, as this is the last IMU measurement we need! - if (imu_data.at(i + 1).timestamp > time1) { - // If we have a very low frequency IMU then, we could have only recorded the first integration (i.e. case 1) and nothing else - // In this case, both the current IMU measurement and the next is greater than the desired intepolation, thus we should just cut the - // current at the desired time Else, we have hit CASE2 and this IMU measurement is not past the desired propagation time, thus add the - // whole IMU reading - if (imu_data.at(i).timestamp > time1 && i == 0) { - // This case can happen if we don't have any imu data that has occured before the startup time - // This means that either we have dropped IMU data, or we have not gotten enough. - // In this case we can't propgate forward in time, so there is not that much we can do. - break; - } else if (imu_data.at(i).timestamp > time1) { - ov_core::ImuData data = interpolate_data(imu_data.at(i - 1), imu_data.at(i), time1); - prop_data.push_back(data); - // PRINT_DEBUG("propagation #%d = CASE 3.1 = %.3f => %.3f\n", - // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); - } else { - prop_data.push_back(imu_data.at(i)); - // PRINT_DEBUG("propagation #%d = CASE 3.2 = %.3f => %.3f\n", - // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); - } - // If the added IMU message doesn't end exactly at the camera time - // Then we need to add another one that is right at the ending time - if (prop_data.at(prop_data.size() - 1).timestamp != time1) { - ov_core::ImuData data = interpolate_data(imu_data.at(i), imu_data.at(i + 1), time1); - prop_data.push_back(data); - // PRINT_DEBUG("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0); - } - break; - } - } - - // Check that we have at least one measurement to propagate with - if (prop_data.empty()) { - if (warn) - PRINT_WARNING( - YELLOW - "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET, - (int)prop_data.size()); - return prop_data; - } - - // If we did not reach the whole integration period (i.e., the last inertial measurement we have is smaller then the time we want to - // reach) Then we should just "stretch" the last measurement to be the whole period (case 3 in the above loop) - // if(time1-imu_data.at(imu_data.size()-1).timestamp > 1e-3) { - // PRINT_DEBUG(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing). - // IMU-CAMERA are likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data; - //} - - // Loop through and ensure we do not have an zero dt values - // This would cause the noise covariance to be Infinity - for (size_t i = 0; i < prop_data.size() - 1; i++) { - if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12) { - if (warn) - PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): Zero DT between IMU reading %d and %d, removing it!\n" RESET, (int)i, - (int)(i + 1)); - prop_data.erase(prop_data.begin() + i); - i--; - } - } - - // Check that we have at least one measurement to propagate with - if (prop_data.size() < 2) { - if (warn) - PRINT_WARNING( - YELLOW - "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET, - (int)prop_data.size()); - return prop_data; - } - - // Success :D - return prop_data; -} - void Propagator::predict_and_compute(std::shared_ptr state, const ov_core::ImuData &data_minus, const ov_core::ImuData &data_plus, Eigen::MatrixXd &F, Eigen::MatrixXd &Qd) { // Time elapsed over interval double dt = data_plus.timestamp - data_minus.timestamp; // assert(data_plus.timestamp>data_minus.timestamp); - + // IMU intrinsic calibration estimates (static) Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); @@ -924,6 +807,123 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d G.block(ba_id, 9, 3, 3) = dt * Eigen::Matrix3d::Identity(); } +std::vector Propagator::select_imu_readings(const std::vector &imu_data, double time0, double time1, + bool warn) { + + // Our vector imu readings + std::vector prop_data; + + // Ensure we have some measurements in the first place! + if (imu_data.empty()) { + if (warn) + PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): No IMU measurements. IMU-CAMERA are likely messed up!!!\n" RESET); + return prop_data; + } + + // Loop through and find all the needed measurements to propagate with + // Note we split measurements based on the given state time, and the update timestamp + for (size_t i = 0; i < imu_data.size() - 1; i++) { + + // START OF THE INTEGRATION PERIOD + // If the next timestamp is greater then our current state time + // And the current is not greater then it yet... + // Then we should "split" our current IMU measurement + if (imu_data.at(i + 1).timestamp > time0 && imu_data.at(i).timestamp < time0) { + ov_core::ImuData data = Propagator::interpolate_data(imu_data.at(i), imu_data.at(i + 1), time0); + prop_data.push_back(data); + // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n", + // (int)i,data.timestamp-prop_data.at(0).timestamp,time0-prop_data.at(0).timestamp); + continue; + } + + // MIDDLE OF INTEGRATION PERIOD + // If our imu measurement is right in the middle of our propagation period + // Then we should just append the whole measurement time to our propagation vector + if (imu_data.at(i).timestamp >= time0 && imu_data.at(i + 1).timestamp <= time1) { + prop_data.push_back(imu_data.at(i)); + // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp); + continue; + } + + // END OF THE INTEGRATION PERIOD + // If the current timestamp is greater then our update time + // We should just "split" the NEXT IMU measurement to the update time, + // NOTE: we add the current time, and then the time at the end of the interval (so we can get a dt) + // NOTE: we also break out of this loop, as this is the last IMU measurement we need! + if (imu_data.at(i + 1).timestamp > time1) { + // If we have a very low frequency IMU then, we could have only recorded the first integration (i.e. case 1) and nothing else + // In this case, both the current IMU measurement and the next is greater than the desired intepolation, thus we should just cut the + // current at the desired time Else, we have hit CASE2 and this IMU measurement is not past the desired propagation time, thus add the + // whole IMU reading + if (imu_data.at(i).timestamp > time1 && i == 0) { + // This case can happen if we don't have any imu data that has occured before the startup time + // This means that either we have dropped IMU data, or we have not gotten enough. + // In this case we can't propgate forward in time, so there is not that much we can do. + break; + } else if (imu_data.at(i).timestamp > time1) { + ov_core::ImuData data = interpolate_data(imu_data.at(i - 1), imu_data.at(i), time1); + prop_data.push_back(data); + // PRINT_DEBUG("propagation #%d = CASE 3.1 = %.3f => %.3f\n", + // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); + } else { + prop_data.push_back(imu_data.at(i)); + // PRINT_DEBUG("propagation #%d = CASE 3.2 = %.3f => %.3f\n", + // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); + } + // If the added IMU message doesn't end exactly at the camera time + // Then we need to add another one that is right at the ending time + if (prop_data.at(prop_data.size() - 1).timestamp != time1) { + ov_core::ImuData data = interpolate_data(imu_data.at(i), imu_data.at(i + 1), time1); + prop_data.push_back(data); + // PRINT_DEBUG("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0); + } + break; + } + } + + // Check that we have at least one measurement to propagate with + if (prop_data.empty()) { + if (warn) + PRINT_WARNING( + YELLOW + "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET, + (int)prop_data.size()); + return prop_data; + } + + // If we did not reach the whole integration period (i.e., the last inertial measurement we have is smaller then the time we want to + // reach) Then we should just "stretch" the last measurement to be the whole period (case 3 in the above loop) + // if(time1-imu_data.at(imu_data.size()-1).timestamp > 1e-3) { + // PRINT_DEBUG(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing). + // IMU-CAMERA are likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data; + //} + + // Loop through and ensure we do not have an zero dt values + // This would cause the noise covariance to be Infinity + for (size_t i = 0; i < prop_data.size() - 1; i++) { + if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12) { + if (warn) + PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): Zero DT between IMU reading %d and %d, removing it!\n" RESET, (int)i, + (int)(i + 1)); + prop_data.erase(prop_data.begin() + i); + i--; + } + } + + // Check that we have at least one measurement to propagate with + if (prop_data.size() < 2) { + if (warn) + PRINT_WARNING( + YELLOW + "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET, + (int)prop_data.size()); + return prop_data; + } + + // Success :D + return prop_data; +} + Eigen::MatrixXd Propagator::compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected) { Eigen::Matrix3d I_3x3 = Eigen::MatrixXd::Identity(3, 3); diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 8da21d2ba..f38f71f08 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -161,6 +161,72 @@ class Propagator { return data; } + /** + * @brief compute the Jacobians for Dw + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * For kalibr model + * \f{align*}{ + * \mathbf{H}_{Dw} & = + * \begin{bmatrix} + * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 + * \end{bmatrix} + * \f} + * + * For rpng model: + * \f{align*}{ + * \mathbf{H}_{Dw} & = + * \begin{bmatrix} + * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * + * @param state Pointer to state + * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed + */ + static Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); + + /** + * @brief compute the Jacobians for Da + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * For kalibr model + * \f{align*}{ + * \mathbf{H}_{Da} & = + * \begin{bmatrix} + * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * + * For rpng: + * \f{align*}{ + * \mathbf{H}_{Da} & = + * \begin{bmatrix} + * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 + * \end{bmatrix} + * \f} + * + * @param state Pointer to state + * @param a_uncorrected Linear acceleration in gyro frame with bias removed + */ + static Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); + + /** + * @brief compute the Jacobians for Tg + * + * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * \f{align*}{ + * \mathbf{H}_{Tg} & = + * \begin{bmatrix} + * {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 + * \end{bmatrix} + * \f} + * + * @param state Pointer to state + * @param a_inI Linear acceleration with bias removed + */ + static Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); + protected: /// Estimate for time offset at last propagation time double last_prop_time_offset = 0.0; @@ -380,71 +446,6 @@ class Propagator { const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, Eigen::MatrixXd &F, Eigen::MatrixXd &G); - /** - * @brief compute the Jacobians for Dw - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * For kalibr model - * \f{align*}{ - * \mathbf{H}_{Dw} & = - * \begin{bmatrix} - * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 - * \end{bmatrix} - * \f} - * - * For rpng model: - * \f{align*}{ - * \mathbf{H}_{Dw} & = - * \begin{bmatrix} - * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * - * @param state Pointer to state - * @param w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed - */ - Eigen::MatrixXd compute_H_Dw(std::shared_ptr state, const Eigen::Vector3d &w_uncorrected); - - /** - * @brief compute the Jacobians for Da - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * For kalibr model - * \f{align*}{ - * \mathbf{H}_{Da} & = - * \begin{bmatrix} - * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * - * For rpng: - * \f{align*}{ - * \mathbf{H}_{Da} & = - * \begin{bmatrix} - * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 - * \end{bmatrix} - * \f} - * - * @param state Pointer to state - * @param a_uncorrected Linear acceleration in gyro frame with bias removed - */ - Eigen::MatrixXd compute_H_Da(std::shared_ptr state, const Eigen::Vector3d &a_uncorrected); - - /** - * @brief compute the Jacobians for Tg - * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * \f{align*}{ - * \mathbf{H}_{Tg} & = - * \begin{bmatrix} - * {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * - * @param state Pointer to state - * @param a_inI Linear acceleration with bias removed - */ - Eigen::MatrixXd compute_H_Tg(std::shared_ptr state, const Eigen::Vector3d &a_inI); /// Container for the noise values NoiseManager _noises; From c6b9f5eb8e0f80054e605e09000cec8d259dc6e1 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 10 Jan 2022 16:49:54 -0500 Subject: [PATCH 34/60] correct imu in zvupt --- ov_msckf/src/state/Propagator.cpp | 10 +++++----- ov_msckf/src/update/UpdaterZeroVelocity.cpp | 11 +++++++++-- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index c9909d745..7e02df759 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -213,15 +213,15 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times } // Now record what the predicted state should be - // TODO: apply IMU intrinsics here to correct the returned angular velocity + // We apply IMU intrinsics here to get the corrected angular velocity state_plus = Eigen::Matrix::Zero(); state_plus.block(0, 0, 4, 1) = state->_imu->quat(); state_plus.block(4, 0, 3, 1) = state->_imu->pos(); state_plus.block(7, 0, 3, 1) = state->_imu->vel(); - if (prop_data.size() > 1) { - state_plus.block(10, 0, 3, 1) = prop_data.at(prop_data.size() - 2).wm - state->_imu->bias_g(); - } else if (!prop_data.empty()) { - state_plus.block(10, 0, 3, 1) = prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g(); + if (!prop_data.empty()) { + Eigen::Vector3d last_a = state->_calib_imu_ACCtoIMU->Rot() * Da * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); + Eigen::Vector3d last_w = state->_calib_imu_GYROtoIMU->Rot() * Dw * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - Tg * last_a); + state_plus.block(10, 0, 3, 1) = last_w; } // Finally replace the imu with the original state we had diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 5bf87b938..eda777aca 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -92,7 +92,13 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest Eigen::VectorXd res = Eigen::VectorXd::Zero(m_size); Eigen::MatrixXd R = Eigen::MatrixXd::Identity(m_size, m_size); + // IMU intrinsic calibration estimates (static) + Eigen::Matrix3d Dw = State::Dm(state->_options.imu_model, state->_calib_imu_dw->value()); + Eigen::Matrix3d Da = State::Dm(state->_options.imu_model, state->_calib_imu_da->value()); + Eigen::Matrix3d Tg = State::Tg(state->_calib_imu_tg->value()); + // Loop through all our IMU and construct the residual and Jacobian + // TODO: should add jacobians here in respect to IMU intrinsics!! // State order is: [q_GtoI, bg, ba, v_IinG] // Measurement order is: [w_true = 0, a_true = 0 or v_k+1 = 0] // w_true = w_m - bw - nw @@ -103,10 +109,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Precomputed values double dt = imu_recent.at(i + 1).timestamp - imu_recent.at(i).timestamp; - Eigen::Vector3d a_hat = imu_recent.at(i).am - state->_imu->bias_a(); + Eigen::Vector3d a_hat = state->_calib_imu_ACCtoIMU->Rot() * Da * (imu_recent.at(i).am - state->_imu->bias_a()); + Eigen::Vector3d w_hat = state->_calib_imu_GYROtoIMU->Rot() * Dw * (imu_recent.at(i).wm - state->_imu->bias_g() - Tg * a_hat); // Measurement residual (true value is zero) - res.block(6 * i + 0, 0, 3, 1) = -(imu_recent.at(i).wm - state->_imu->bias_g()); + res.block(6 * i + 0, 0, 3, 1) = -w_hat; if (!integrated_accel_constraint) { res.block(6 * i + 3, 0, 3, 1) = -(a_hat - state->_imu->Rot() * _gravity); } else { From fa70006d7f081b50f64d8184e3b650e4a58fc507 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 11 Jan 2022 09:42:17 -0500 Subject: [PATCH 35/60] start updating prop docs --- docs/propagation-analytical.dox | 8 + docs/propagation-discrete.dox | 323 ++++++++++++++++++++++++++++ docs/propagation.dox | 268 +++-------------------- ov_eval/src/calc/ResultSimulation.h | 2 +- ov_msckf/src/state/Propagator.h | 2 +- 5 files changed, 360 insertions(+), 243 deletions(-) create mode 100644 docs/propagation-analytical.dox create mode 100644 docs/propagation-discrete.dox diff --git a/docs/propagation-analytical.dox b/docs/propagation-analytical.dox new file mode 100644 index 000000000..f819c5ab8 --- /dev/null +++ b/docs/propagation-analytical.dox @@ -0,0 +1,8 @@ +/** + +@page propagation_analytical Analytical Propagation +@tableofcontents + + + +*/ \ No newline at end of file diff --git a/docs/propagation-discrete.dox b/docs/propagation-discrete.dox new file mode 100644 index 000000000..3d0bf2f6f --- /dev/null +++ b/docs/propagation-discrete.dox @@ -0,0 +1,323 @@ +/** + +@page propagation_discrete Discrete Propagation +@tableofcontents + + +@section discrete_imu_measurements IMU Measurements + +We use a 6-axis inertial measurement unit (IMU) to propagate the inertial navigation system (INS), +which provides measurements of the local rotational velocity (angular rate) \f$\boldsymbol{\omega}_m\f$ and +local translational acceleration \f$\mathbf{a}_m\f$: + +\f{align*}{ + \boldsymbol{\omega}_m(t) &= \boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ + \mathbf{a}_m(t) &= \mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) +\f} + +where \f$\boldsymbol{\omega}\f$ and \f$\mathbf{a}\f$ are the true rotational velocity and translational +acceleration in the IMU local frame \f$\{I\}\f$, +\f$\mathbf{b}_{g}\f$ and \f$\mathbf{b}_{a}\f$ are the gyroscope and accelerometer biases, and \f$\mathbf{n}_{{g}}\f$ +\f$\mathbf{n}_{{a}}\f$ are white Gaussian noise, +\f${^G\mathbf{g}} = [ 0 ~~ 0 ~~ 9.81 ]^\top\f$ is the gravity expressed in the global frame \f$\{G\}\f$ +(noting that the gravity is slightly different on different locations of the globe), +and \f$^I_G\mathbf{R}\f$ is the rotation matrix from global to IMU local frame. + + +@section discrete_ins_state State Vector + +We define our INS state vector \f$\mathbf{x}_I\f$ at time \f$t\f$ as: + +\f{align*}{ +\mathbf{x}_I(t) = +\begin{bmatrix} +^I_G\bar{q}(t) \\ +^G\mathbf{p}_I(t) \\ +^G\mathbf{v}_I(t)\\ +\mathbf{b}_{\mathbf{g}}(t) \\ +\mathbf{b}_{\mathbf{a}}(t) +\end{bmatrix} +\f} + +where \f$^I_G\bar{q}\f$ is the unit quaternion representing the rotation global to IMU frame, +\f$^G\mathbf{p}_I\f$ is the position of IMU in global frame, +and \f$^G\mathbf{v}_I\f$ is the velocity of IMU in global frame. +We will often write time as a subscript of \f$I\f$ describing the state of IMU at the time +for notation clarity (e.g., \f$^{I_t}_G\bar{q} = \text{}^I_G\bar{q}(t)\f$). +In order to define the IMU error state, the standard additive error definition is employed +for the position, velocity, and biases, +while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quaternion multiplicative error +\f$\otimes\f$: + +\f{align*}{ + ^I_G\bar{q} &= \delta \bar{q} \otimes \text{}^I_G \hat{\bar{q}}\\ + \delta \bar{q} &= \begin{bmatrix} + \hat{\mathbf{k}}\sin(\frac{1}{2}\tilde{\theta})\\ + \cos(\frac{1}{2}\tilde{\theta}) \end{bmatrix} + \simeq + \begin{bmatrix} + \frac{1}{2}\tilde{\boldsymbol{\theta}}\\ 1 + \end{bmatrix} +\f} + +where \f$\hat{\mathbf{k}}\f$ is the rotation axis and \f$\tilde{\theta}\f$ is the rotation angle. +For small rotation, the error angle vector is approximated by \f$\tilde{\boldsymbol{\theta}} = \tilde{\theta}~\hat{\mathbf{k}}\f$ +as the error vector about the three orientation axes. +The total IMU error state thus is defined as the following 15x1 (not 16x1) vector: + +\f{align*}{ +\tilde{\mathbf{x}}_I(t) = +\begin{bmatrix} +^I_G\tilde{\boldsymbol{\theta}}(t) \\ +^G\tilde{\mathbf{p}}_I(t) \\ +^G\tilde{\mathbf{v}}_I(t)\\ +\tilde{\mathbf{b}}_{{g}}(t) \\ +\tilde{\mathbf{b}}_{{a}}(t) +\end{bmatrix} +\f} + + + +@section disc_prop Discrete-time IMU Propagation + + +A simpler method is to model the measurements as discrete-time over the integration period. +To do this, the measurements can be assumed to be constant during the sampling period. +We employ this assumption and approximate that the measurement at time \f$t_k\f$ remains +the same until we get the next measurement at \f$t_{k+1}\f$. +For the quaternion propagation, it is the same as continuous-time propagation +with constant measurement assumption \f${\boldsymbol{\omega}}_{m}(t_k) = {\boldsymbol{\omega}}_{m,k}\f$. +We use subscript \f$k\f$ to denote it is the measurement we get at time \f$t_k\f$. +Therefore the propagation of quaternion can be written as: + +\f{align*}{ + \text{}^{I_{k+1}}_{G}\hat{\bar{q}} + = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) + \text{}^{I_{k}}_{G}\hat{\bar{q}} +\f} + +For the velocity and position propagation we have constant +\f$\mathbf{a}_{m}(t_k) = \mathbf{a}_{m,k}\f$ over \f$t \in [t_k, t_{k+1}]\f$. +We can therefore directly solve for the new states as: + +\f{align*}{ + ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t + +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ + + ^G\hat{\mathbf{p}}_{I_{k+1}} + &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 +\f} + +The propagation of each bias is likewise the continuous system: + +\f{align*}{ + \hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k}\\ + \hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} +\f} + + + + + + + +@section error_prop Discrete-time Error-state Propagation + +In order to propagate the covariance matrix, we should derive the error-state propagation, +i.e., computing the system Jacobian \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and noise Jacobian \f$\mathbf{G}_{k}\f$. +In particular, when the covariance matrix of the continuous-time measurement noises is given by +\f$\mathbf{Q}_c\f$, then the discrete-time noise covariance \f$\mathbf{Q}_d\f$ can be computed as +(see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR Eq. (129) and (130)): + +\f{align*}{ +\sigma_{g} &= \frac{1}{\sqrt{\Delta t}}~ \sigma_{g_c} \\ +\sigma_{bg} &= \sqrt{\Delta t}~ \sigma_{bg_c} \\[1em] +\mathbf{Q}_{meas} &= +\begin{bmatrix} +\frac{1}{\Delta t}~ \sigma_{g_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ +\mathbf{0}_3 & \frac{1}{\Delta t}~ \sigma_{a_c}^2~ \mathbf{I}_3 +\end{bmatrix} \\ +\mathbf{Q}_{bias} &= +\begin{bmatrix} +\Delta t~ \sigma_{bg_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ +\mathbf{0}_3 & \Delta t~ \sigma_{ba_c}^2~ \mathbf{I}_3 +\end{bmatrix} +\f} + +where \f$\mathbf{n} = [ \mathbf{n}_g ~ \mathbf{n}_a ~ \mathbf{n}_{bg} ~ \mathbf{n}_{ba} ]^\top\f$ are the discrete +IMU sensor noises which have been converted from their continuous representations. +We define the stacked discrete measurement noise as follows: + +\f{align*}{ +\mathbf{Q}_{d} &= +\begin{bmatrix} +\mathbf{Q}_{meas} & \mathbf{0}_3 \\ +\mathbf{0}_3 & \mathbf{Q}_{bias} +\end{bmatrix} +\f} + +The method of computing Jacobians is to "perturb" each variable in the system and see how the old error "perturbation" relates to the new error state. +That is, +\f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ can be found by perturbing each variable as: +\f{align*}{ +\tilde{\mathbf{x}}_I(t_{k+1}) = \boldsymbol{\Phi}(t_{k+1},t_k) \tilde{\mathbf{x}}_I(t_{k}) + \mathbf{G}_{k} \mathbf{n} +\f} +For the orientation error propagation, we start with the \f$\mathbf{SO}(3)\f$ perturbation using +\f${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}\f$: + +\f{align*}{ +{}^{I_{k+1}}_G \mathbf{R} &= \text{}^{I_{k+1}}_{I_{k}} \mathbf{R} \text{}^{I_{k}}_G \mathbf{R} \\ +(\mathbf{I}_3 - \lfloor ^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k+1}}_{G} +\hat{\mathbf{R}} +&\approx \textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) +(\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} +\hat{\mathbf{R}}\\ +&=\textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\textrm{exp}(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) +(\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} +\hat{\mathbf{R}}\\ +&=\text{}^{I_{k+1}}_{I_{k}} \hat{\mathbf{R}} + (\mathbf{I}_3 - \lfloor \mathbf J_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t) + \tilde{\boldsymbol{\omega}}_k\Delta t \times\rfloor) +(\mathbf{I}_3 - \lfloor ^{I_k}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) +\text{}^{I_{k}}_G \hat{\mathbf{R}} +\f} + +where \f$\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} - \hat{\boldsymbol{\omega}} += -(\tilde{\mathbf{b}}_{\mathbf{g}} + \mathbf{n}_g)\f$ handles both the perturbation to the bias and measurement noise. +\f$\mathbf {J}_r(\boldsymbol{\theta})\f$ is the right Jacobian of \f$\mathbf{SO}(3)\f$ +that maps the variation of rotation angle in the parameter vector space into the variation in the tangent vector space to the manifold +[see @ref ov_core::Jr_so3()]. +By neglecting the second order terms from above, we obtain the following orientation error propagation: + +\f{align*} +\text{}^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}} \approx +\text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} \text{}^{I_k}_{G}\tilde{\boldsymbol{\theta}} +- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) +\Delta t (\tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{\mathbf{g},k}) +\f} + +Now we can do error propagation of position and velocity using the same scheme: + +\f{align*}{ +^G\mathbf{p}_{I_{k+1}} + &= \text{}^G\mathbf{p}_{I_k} + \text{}^G\mathbf{v}_{I_k} \Delta t + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + + \frac{1}{2}\text{}^{I_k}_G\mathbf{R}^\top \mathbf{a}_{k}\Delta t^2\\ + +^G\hat{\mathbf{p}}_{I_{k+1}} + \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} + &\approx \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\tilde{\mathbf{p}}_{I_k} + + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t + + \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2\\ + &\hspace{4cm} + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top + (\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) + (\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t^2\\ +\\ +^G\mathbf{v}_{k+1} &= \text{}^G\mathbf{v}_{I_k} - {}^G\mathbf{g}\Delta t ++\text{}^{I_k}_G\mathbf{R}^\top\mathbf{a}_{k}\Delta t\\ + +^G\hat{\mathbf{v}}_{k+1} + ^G\tilde{\mathbf{v}}_{k+1} &\approx +{}^G\hat{\mathbf{v}}_{I_k} + {}^G\tilde{\mathbf{v}}_{I_k} +- {}^G\mathbf{g}\Delta t ++ \text{}^{I_k}_G\hat{\mathbf{R}}^\top +(\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) +(\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t +\f} + +where \f$\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}} + = - (\tilde{\mathbf{b}}_{\mathbf{a}} + \mathbf{n}_{\mathbf{a}})\f$. +By neglecting the second order error terms, we obtain the following position and velocity error propagation: + +\f{align*}{ +\text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &= +\text{}^G\tilde{\mathbf{p}}_{I_k} ++ \Delta t \text{}^G\tilde{\mathbf{v}}_{I_k} +- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top +\lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor +^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} +- \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 +(\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k})\\ +^G\tilde{\mathbf{v}}_{k+1} &= +\text{}^G\tilde{\mathbf{v}}_{I_k} + - \text{}^{I_k}_G\hat{\mathbf{R}}^\top + \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor + ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} + - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t + (\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k}) +\f} + +The propagation of the two random-walk biases are as follows: + +\f{align*}{ + \mathbf{b}_{\mathbf{g},k+1} &= \mathbf{b}_{\mathbf{g},k} + \mathbf{n}_{wg} \\ + \hat{\mathbf{b}}_{\mathbf{g},k+1} + \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= + \hat{\mathbf{b}}_{\mathbf{g},k} + \tilde{\mathbf{b}}_{\mathbf{g},k} + + \mathbf{n}_{wg} \\ + \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= + \tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{wg} \\[1em] + +\mathbf{b}_{\mathbf{a},k+1} &= \mathbf{b}_{\mathbf{a},k} + \mathbf{n}_{wa} \\ + \hat{\mathbf{b}}_{\mathbf{a},k+1} + \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= + \hat{\mathbf{b}}_{\mathbf{a},k} + \tilde{\mathbf{b}}_{\mathbf{a},k} + + \mathbf{n}_{wa} \\ + \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= + \tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{wa} +\f} + +By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices as: + + +\f{align*}{ +\boldsymbol{\Phi}(t_{k+1},t_k) &= +\begin{bmatrix} +\text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} & \mathbf{0}_3 & \mathbf{0}_3 & +- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) +\Delta t & \mathbf{0}_3 \\ + +- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor +& \mathbf{I}_3 & \Delta t \mathbf{I}_3 & \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 \\ + +- \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor +& \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t \\ + +\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ +\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 +\end{bmatrix} +\f} + + + +\f{align*}{ +\mathbf{G}_{k} &= +\begin{bmatrix} +- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) +\Delta t & \mathbf{0}_3 +& \mathbf{0}_3 & \mathbf{0}_3 \\ + +\mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 +& \mathbf{0}_3 & \mathbf{0}_3 \\ + +\mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t +& \mathbf{0}_3 & \mathbf{0}_3 \\ + +\mathbf{0}_3 & \mathbf{0}_3 +& \mathbf{I}_3 & \mathbf{0}_3 \\ + +\mathbf{0}_3 & \mathbf{0}_3 +& \mathbf{0}_3 & \mathbf{I}_3 +\end{bmatrix} +\f} + + + + +Now, with the computed \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices, +we can propagate the covariance from \f$t_k\f$ to \f$t_{k+1}\f$: +\f{align*}{ +\mathbf{P}_{k+1|k} = \boldsymbol{\Phi}(t_{k+1},t_k)\mathbf{P}_{k|k}\boldsymbol{\Phi}(t_{k+1},t_k)^\top + \mathbf{G}_k\mathbf{Q}_d\mathbf{G}_k^\top +\f} + + +*/ \ No newline at end of file diff --git a/docs/propagation.dox b/docs/propagation.dox index 1846a373c..59d7f94b6 100644 --- a/docs/propagation.dox +++ b/docs/propagation.dox @@ -21,10 +21,32 @@ acceleration in the IMU local frame \f$\{I\}\f$, \f$\mathbf{n}_{{a}}\f$ are white Gaussian noise, \f${^G\mathbf{g}} = [ 0 ~~ 0 ~~ 9.81 ]^\top\f$ is the gravity expressed in the global frame \f$\{G\}\f$ (noting that the gravity is slightly different on different locations of the globe), -and \f$^I_G\mathbf{R}\f$ is the rotation matrix from global to IMU local frame. +and \f$^I_G\mathbf{R}\f$ is the rotation matrix from global to IMU local frame. + +A more complete and complex model which also takes into account the intrinsic properties of the IMU can be considered. +[MEMS IMU](https://en.wikipedia.org/wiki/Vibrating_structure_gyroscope#MEMS_gyroscopes) typically have non-perfect readings which would invalidate the above equations. +We can define additional sensor frames \f$\{w\}\f$ and \f$\{a\}\f$ which can be related back to the "ideal" inertial frame \f$\{I\}\f$ that we wish to estimate. +Thus we can define the following: + +\f{align*}{ +{}^w\boldsymbol{\omega}_{m}(t) & = + \mathbf{T}_{w} {}^w_I\mathbf{R} {}^I\boldsymbol{\omega}(t) + + \mathbf{T}_{g} {}^I\mathbf{a}(t) + \mathbf{b}_g(t) + \mathbf{n}_g(t) \\ +{}^a\mathbf{a}_{m}(t) & = \mathbf{T}_a {}^a_I\mathbf{R} {}^I\mathbf{a}(t) + + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_a(t) + \mathbf{n}_a(t) +\f} + +where \f$\mathbf{T}_{w}\f$ and \f$\mathbf{T}_a\f$ are invertible \f$3\times3\f$ matrices which represent the +scale imperfection and axis misalignment for \f$\{w\}\f$ and \f$\{a\}\f$, respectively. +\f${}^w_I\mathbf{R}\f$ and \f${}^a_I\mathbf{R}\f$ denote the rotation from the gyroscope frame and +acceleration frame to base "inertial" frame \f$\{I\}\f$ that can be determined to coincide with either. +\f$\mathbf{T}_g\f$ denotes the gravity sensitivity, which represents the effects of gravity to the +gyroscope readings due to its inherent inertia. + -@section ins_state State Vector + +@section ins_state Inertial State Vector We define our INS state vector \f$\mathbf{x}_I\f$ at time \f$t\f$ as: @@ -131,7 +153,6 @@ Note that the above kinematics have been defined in terms of the *true* accelera - @section conti_prop Continuous-time IMU Propagation @@ -227,247 +248,12 @@ All of the above integrals could be analytically or numerically solved if one wi - -@section disc_prop Discrete-time IMU Propagation +@section prop_integration Mean and Uncertainty Integration -A simpler method is to model the measurements as discrete-time over the integration period. -To do this, the measurements can be assumed to be constant during the sampling period. -We employ this assumption and approximate that the measurement at time \f$t_k\f$ remains -the same until we get the next measurement at \f$t_{k+1}\f$. -For the quaternion propagation, it is the same as continuous-time propagation -with constant measurement assumption \f${\boldsymbol{\omega}}_{m}(t_k) = {\boldsymbol{\omega}}_{m,k}\f$. -We use subscript \f$k\f$ to denote it is the measurement we get at time \f$t_k\f$. -Therefore the propagation of quaternion can be written as: - -\f{align*}{ - \text{}^{I_{k+1}}_{G}\hat{\bar{q}} - = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) - \text{}^{I_{k}}_{G}\hat{\bar{q}} -\f} - -For the velocity and position propagation we have constant -\f$\mathbf{a}_{m}(t_k) = \mathbf{a}_{m,k}\f$ over \f$t \in [t_k, t_{k+1}]\f$. -We can therefore directly solve for the new states as: - -\f{align*}{ - ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t - +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ - - ^G\hat{\mathbf{p}}_{I_{k+1}} - &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 - + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 -\f} - -The propagation of each bias is likewise the continuous system: - -\f{align*}{ - \hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k}\\ - \hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} -\f} +- @subpage propagation_discrete --- Simplified discrete mean and covariance propagation derivations +- @subpage propagation_analytical --- Analytical mean and covariance propagation with IMU intriniscs derivations - - - - -@section error_prop Discrete-time Error-state Propagation - -In order to propagate the covariance matrix, we should derive the error-state propagation, -i.e., computing the system Jacobian \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and noise Jacobian \f$\mathbf{G}_{k}\f$. -In particular, when the covariance matrix of the continuous-time measurement noises is given by -\f$\mathbf{Q}_c\f$, then the discrete-time noise covariance \f$\mathbf{Q}_d\f$ can be computed as -(see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR Eq. (129) and (130)): - -\f{align*}{ -\sigma_{g} &= \frac{1}{\sqrt{\Delta t}}~ \sigma_{g_c} \\ -\sigma_{bg} &= \sqrt{\Delta t}~ \sigma_{bg_c} \\[1em] -\mathbf{Q}_{meas} &= -\begin{bmatrix} -\frac{1}{\Delta t}~ \sigma_{g_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ -\mathbf{0}_3 & \frac{1}{\Delta t}~ \sigma_{a_c}^2~ \mathbf{I}_3 -\end{bmatrix} \\ -\mathbf{Q}_{bias} &= -\begin{bmatrix} -\Delta t~ \sigma_{bg_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ -\mathbf{0}_3 & \Delta t~ \sigma_{ba_c}^2~ \mathbf{I}_3 -\end{bmatrix} -\f} - -where \f$\mathbf{n} = [ \mathbf{n}_g ~ \mathbf{n}_a ~ \mathbf{n}_{bg} ~ \mathbf{n}_{ba} ]^\top\f$ are the discrete -IMU sensor noises which have been converted from their continuous representations. -We define the stacked discrete measurement noise as follows: - -\f{align*}{ -\mathbf{Q}_{d} &= -\begin{bmatrix} -\mathbf{Q}_{meas} & \mathbf{0}_3 \\ -\mathbf{0}_3 & \mathbf{Q}_{bias} -\end{bmatrix} -\f} - -The method of computing Jacobians is to "perturb" each variable in the system and see how the old error "perturbation" relates to the new error state. -That is, -\f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ can be found by perturbing each variable as: -\f{align*}{ -\tilde{\mathbf{x}}_I(t_{k+1}) = \boldsymbol{\Phi}(t_{k+1},t_k) \tilde{\mathbf{x}}_I(t_{k}) + \mathbf{G}_{k} \mathbf{n} -\f} -For the orientation error propagation, we start with the \f$\mathbf{SO}(3)\f$ perturbation using -\f${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}\f$: - -\f{align*}{ -{}^{I_{k+1}}_G \mathbf{R} &= \text{}^{I_{k+1}}_{I_{k}} \mathbf{R} \text{}^{I_{k}}_G \mathbf{R} \\ -(\mathbf{I}_3 - \lfloor ^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k+1}}_{G} -\hat{\mathbf{R}} -&\approx \textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) -(\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} -\hat{\mathbf{R}}\\ -&=\textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\textrm{exp}(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) -(\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} -\hat{\mathbf{R}}\\ -&=\text{}^{I_{k+1}}_{I_{k}} \hat{\mathbf{R}} - (\mathbf{I}_3 - \lfloor \mathbf J_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t) - \tilde{\boldsymbol{\omega}}_k\Delta t \times\rfloor) -(\mathbf{I}_3 - \lfloor ^{I_k}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) -\text{}^{I_{k}}_G \hat{\mathbf{R}} -\f} - -where \f$\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} - \hat{\boldsymbol{\omega}} -= -(\tilde{\mathbf{b}}_{\mathbf{g}} + \mathbf{n}_g)\f$ handles both the perturbation to the bias and measurement noise. -\f$\mathbf {J}_r(\boldsymbol{\theta})\f$ is the right Jacobian of \f$\mathbf{SO}(3)\f$ -that maps the variation of rotation angle in the parameter vector space into the variation in the tangent vector space to the manifold -[see @ref ov_core::Jr_so3()]. -By neglecting the second order terms from above, we obtain the following orientation error propagation: - -\f{align*} -\text{}^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}} \approx -\text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} \text{}^{I_k}_{G}\tilde{\boldsymbol{\theta}} -- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t (\tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{\mathbf{g},k}) -\f} - -Now we can do error propagation of position and velocity using the same scheme: - -\f{align*}{ -^G\mathbf{p}_{I_{k+1}} - &= \text{}^G\mathbf{p}_{I_k} + \text{}^G\mathbf{v}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 - + \frac{1}{2}\text{}^{I_k}_G\mathbf{R}^\top \mathbf{a}_{k}\Delta t^2\\ - -^G\hat{\mathbf{p}}_{I_{k+1}} + \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} - &\approx \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\tilde{\mathbf{p}}_{I_k} - + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t - + \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2\\ - &\hspace{4cm} + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top - (\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) - (\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t^2\\ -\\ -^G\mathbf{v}_{k+1} &= \text{}^G\mathbf{v}_{I_k} - {}^G\mathbf{g}\Delta t -+\text{}^{I_k}_G\mathbf{R}^\top\mathbf{a}_{k}\Delta t\\ - -^G\hat{\mathbf{v}}_{k+1} + ^G\tilde{\mathbf{v}}_{k+1} &\approx -{}^G\hat{\mathbf{v}}_{I_k} + {}^G\tilde{\mathbf{v}}_{I_k} -- {}^G\mathbf{g}\Delta t -+ \text{}^{I_k}_G\hat{\mathbf{R}}^\top -(\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) -(\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t -\f} - -where \f$\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}} - = - (\tilde{\mathbf{b}}_{\mathbf{a}} + \mathbf{n}_{\mathbf{a}})\f$. -By neglecting the second order error terms, we obtain the following position and velocity error propagation: - -\f{align*}{ -\text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &= -\text{}^G\tilde{\mathbf{p}}_{I_k} -+ \Delta t \text{}^G\tilde{\mathbf{v}}_{I_k} -- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top -\lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor -^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} -- \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 -(\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k})\\ -^G\tilde{\mathbf{v}}_{k+1} &= -\text{}^G\tilde{\mathbf{v}}_{I_k} - - \text{}^{I_k}_G\hat{\mathbf{R}}^\top - \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor - ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} - - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t - (\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k}) -\f} - -The propagation of the two random-walk biases are as follows: - -\f{align*}{ - \mathbf{b}_{\mathbf{g},k+1} &= \mathbf{b}_{\mathbf{g},k} + \mathbf{n}_{wg} \\ - \hat{\mathbf{b}}_{\mathbf{g},k+1} + \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= - \hat{\mathbf{b}}_{\mathbf{g},k} + \tilde{\mathbf{b}}_{\mathbf{g},k} - + \mathbf{n}_{wg} \\ - \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= - \tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{wg} \\[1em] - -\mathbf{b}_{\mathbf{a},k+1} &= \mathbf{b}_{\mathbf{a},k} + \mathbf{n}_{wa} \\ - \hat{\mathbf{b}}_{\mathbf{a},k+1} + \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= - \hat{\mathbf{b}}_{\mathbf{a},k} + \tilde{\mathbf{b}}_{\mathbf{a},k} - + \mathbf{n}_{wa} \\ - \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= - \tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{wa} -\f} - -By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices as: - - -\f{align*}{ -\boldsymbol{\Phi}(t_{k+1},t_k) &= -\begin{bmatrix} -\text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} & \mathbf{0}_3 & \mathbf{0}_3 & -- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t & \mathbf{0}_3 \\ - -- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor -& \mathbf{I}_3 & \Delta t \mathbf{I}_3 & \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 \\ - -- \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor -& \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t \\ - -\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ -\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 -\end{bmatrix} -\f} - - - -\f{align*}{ -\mathbf{G}_{k} &= -\begin{bmatrix} -- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t & \mathbf{0}_3 -& \mathbf{0}_3 & \mathbf{0}_3 \\ - -\mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 -& \mathbf{0}_3 & \mathbf{0}_3 \\ - -\mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t -& \mathbf{0}_3 & \mathbf{0}_3 \\ - -\mathbf{0}_3 & \mathbf{0}_3 -& \mathbf{I}_3 & \mathbf{0}_3 \\ - -\mathbf{0}_3 & \mathbf{0}_3 -& \mathbf{0}_3 & \mathbf{I}_3 -\end{bmatrix} -\f} - - - - -Now, with the computed \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices, -we can propagate the covariance from \f$t_k\f$ to \f$t_{k+1}\f$: -\f{align*}{ -\mathbf{P}_{k+1|k} = \boldsymbol{\Phi}(t_{k+1},t_k)\mathbf{P}_{k|k}\boldsymbol{\Phi}(t_{k+1},t_k)^\top + \mathbf{G}_k\mathbf{Q}_d\mathbf{G}_k^\top -\f} - - */ \ No newline at end of file diff --git a/ov_eval/src/calc/ResultSimulation.h b/ov_eval/src/calc/ResultSimulation.h index 8acb5416f..415836d95 100644 --- a/ov_eval/src/calc/ResultSimulation.h +++ b/ov_eval/src/calc/ResultSimulation.h @@ -100,7 +100,7 @@ class ResultSimulation { * @param doplotting True if you want to display the plots * @param max_time Max number of second we want to plot */ - void plot_imu_intrinsics(bool doploting, double max_time = INFINITY); + void plot_imu_intrinsics(bool doplotting, double max_time = INFINITY); protected: // Trajectory data (loaded from file and timestamp intersected) diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index f38f71f08..f804af2a7 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -275,7 +275,7 @@ class Propagator { * @param new_v The resulting new velocity after integration * @param new_p The resulting new position after integration */ - void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &w_hat, + void predict_mean_discrete(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p); /** From 88e2c4d44f9ede12887a5abe760a56b472fde875 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 11 Jan 2022 15:53:21 -0500 Subject: [PATCH 36/60] more propagation doc updates --- Doxyfile | 2 +- docs/bib/extra.bib | 461 +++++++++++++++++--------------- docs/dev-roadmap.dox | 7 +- docs/propagation-analytical.dox | 20 ++ docs/propagation-discrete.dox | 203 +++++++------- docs/propagation.dox | 202 +++++--------- ov_core/src/utils/quat_ops.h | 12 +- 7 files changed, 444 insertions(+), 463 deletions(-) diff --git a/Doxyfile b/Doxyfile index f47105fcc..fd27bd165 100644 --- a/Doxyfile +++ b/Doxyfile @@ -1764,7 +1764,7 @@ LATEX_BIB_STYLE = plainnat # The default value is: NO. # This tag requires that the tag GENERATE_LATEX is set to YES. -LATEX_TIMESTAMP = NO +LATEX_TIMESTAMP = YES #--------------------------------------------------------------------------- # Configuration options related to the RTF output diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index f3d81164b..bc946e582 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -1,241 +1,274 @@ @article{Patron2015IJCV, - title = {A spline-based trajectory representation for sensor fusion and rolling shutter cameras}, - author = {Patron-Perez, Alonso and Lovegrove, Steven and Sibley, Gabe}, - journal = {International Journal of Computer Vision}, - volume = {113}, - number = {3}, - pages = {208--219}, - year = {2015}, - publisher = {Springer} -} - + title = {A spline-based trajectory representation for sensor fusion and rolling shutter cameras}, + author = {Patron-Perez, Alonso and Lovegrove, Steven and Sibley, Gabe}, + year = 2015, + journal = {International Journal of Computer Vision}, + publisher = {Springer}, + volume = 113, + number = 3, + pages = {208--219} +} @article{Mueggler2018TRO, - Title = {Continuous-Time Visual-Inertial Odometry for Event Cameras}, - Author = {E. Mueggler and G. Gallego and H. Rebecq and D. Scaramuzza}, - Journal = {IEEE Transactions on Robotics}, - Year = {2018}, - Pages = {1-16}, - url = {http://rpg.ifi.uzh.ch/docs/TRO18_Mueggler.pdf}, -} - - + title = {Continuous-Time Visual-Inertial Odometry for Event Cameras}, + author = {E. Mueggler and G. Gallego and H. Rebecq and D. Scaramuzza}, + year = 2018, + journal = {IEEE Transactions on Robotics}, + pages = {1--16}, + url = {http://rpg.ifi.uzh.ch/docs/TRO18_Mueggler.pdf} +} @article{Burri2016IJRR, - title = {The EuRoC micro aerial vehicle datasets}, - author = {Burri, Michael and Nikolic, Janosch and Gohl, Pascal and Schneider, Thomas and Rehder, Joern and Omari, Sammy and Achtelik, Markus W and Siegwart, Roland}, - journal = {The International Journal of Robotics Research}, - volume = {35}, - number = {10}, - pages = {1157--1163}, - year = {2016}, - publisher = {SAGE Publications Sage UK: London, England} -} - + title = {The EuRoC micro aerial vehicle datasets}, + author = {Burri, Michael and Nikolic, Janosch and Gohl, Pascal and Schneider, Thomas and Rehder, Joern and Omari, Sammy and Achtelik, Markus W and Siegwart, Roland}, + year = 2016, + journal = {The International Journal of Robotics Research}, + publisher = {SAGE Publications Sage UK: London, England}, + volume = 35, + number = 10, + pages = {1157--1163} +} @inproceedings{Schubert2018IROS, - title = {The TUM VI benchmark for evaluating visual-inertial odometry}, - author = {Schubert, David and Goll, Thore and Demmel, Nikolaus and Usenko, Vladyslav and St { \"u } ckler, J { \"o } rg and Cremers, Daniel}, - booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, - pages = {1680--1687}, - year = {2018}, - organization = {IEEE}, - url = {https://arxiv.org/pdf/1804.06120.pdf}, -} - + title = {The TUM VI benchmark for evaluating visual-inertial odometry}, + author = {Schubert, David and Goll, Thore and Demmel, Nikolaus and Usenko, Vladyslav and St { \"u } ckler, J { \"o } rg and Cremers, Daniel}, + year = 2018, + booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pages = {1680--1687}, + url = {https://arxiv.org/pdf/1804.06120.pdf}, + organization = {IEEE} +} @inproceedings{Furgale2013IROS, - title = {Unified temporal and spatial calibration for multi-sensor systems}, - author = {Furgale, Paul and Rehder, Joern and Siegwart, Roland}, - booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems}, - pages = {1280--1286}, - year = {2013}, + title = {Unified temporal and spatial calibration for multi-sensor systems}, + author = {Furgale, Paul and Rehder, Joern and Siegwart, Roland}, + year = 2013, + booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems}, + pages = {1280--1286}, organization = {IEEE} } - @article{Trawny2005TR, - title = {Indirect Kalman filter for 3D attitude estimation}, - author = {Trawny, Nikolas and Roumeliotis, Stergios I}, - journal = {University of Minnesota, Dept. of Comp. Sci. \& Eng., Tech. Rep}, - volume = {2}, - pages = {2005}, - year = {2005}, - url = {http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf}, -} - - + title = {Indirect Kalman filter for 3D attitude estimation}, + author = {Trawny, Nikolas and Roumeliotis, Stergios I}, + year = 2005, + journal = {University of Minnesota, Dept. of Comp. Sci. \& Eng., Tech. Rep}, + volume = 2, + pages = 2005, + url = {http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf} +} @book{Maybeck1982STOC, - title = {Stochastic models, estimation, and control}, - author = {Maybeck, Peter S}, - volume = {3}, - year = {1982}, - publisher = {Academic press}, - url = {https://books.google.com/books?id=L_YVMUJKNQUC}, -} - + title = {Stochastic models, estimation, and control}, + author = {Maybeck, Peter S}, + year = 1982, + publisher = {Academic press}, + volume = 3, + url = {https://books.google.com/books?id=L_YVMUJKNQUC} +} @article{Huang2010IJRR, - title = {Observability-based rules for designing consistent EKF SLAM estimators}, - author = {Huang, Guoquan P and Mourikis, Anastasios I and Roumeliotis, Stergios I}, - journal = {The International Journal of Robotics Research}, - volume = {29}, - number = {5}, - pages = {502--528}, - year = {2010}, - publisher = {SAGE Publications Sage UK: London, England}, - url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.228.5851&rep=rep1&type=pdf}, -} - + title = {Observability-based rules for designing consistent EKF SLAM estimators}, + author = {Huang, Guoquan P and Mourikis, Anastasios I and Roumeliotis, Stergios I}, + year = 2010, + journal = {The International Journal of Robotics Research}, + publisher = {SAGE Publications Sage UK: London, England}, + volume = 29, + number = 5, + pages = {502--528}, + url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.228.5851&rep=rep1&type=pdf} +} @article{Li2013IJRR, - title = {High-precision, consistent EKF-based visual-inertial odometry}, - author = {Li, Mingyang and Mourikis, Anastasios I}, - journal = {The International Journal of Robotics Research}, - volume = {32}, - number = {6}, - pages = {690--711}, - year = {2013}, - publisher = {SAGE Publications Sage UK: London, England} -} - + title = {High-precision, consistent EKF-based visual-inertial odometry}, + author = {Li, Mingyang and Mourikis, Anastasios I}, + year = 2013, + journal = {The International Journal of Robotics Research}, + publisher = {SAGE Publications Sage UK: London, England}, + volume = 32, + number = 6, + pages = {690--711} +} @book{Kay1993, - title = {Fundamentals of statistical signal processing}, - author = {Kay, Steven M}, - year = {1993}, - publisher = {Prentice Hall PTR}, - url = {http://users.isr.ist.utl.pt/~pjcro/temp/Fundamentals\%20Of\%20Statistical\%20Signal\%20Processing\%2D\%2DEstimation\%20Theory-Kay.pdf}, + title = {Fundamentals of statistical signal processing}, + author = {Kay, Steven M}, + year = 1993, + publisher = {Prentice Hall PTR}, + url = {http://users.isr.ist.utl.pt/~pjcro/temp/Fundamentals\%20Of\%20Statistical\%20Signal\%20Processing\%2D\%2DEstimation\%20Theory-Kay.pdf} } - @inproceedings{Mourikis2007ICRA, - title = {A multi-state constraint Kalman filter for vision-aided inertial navigation}, - author = {Mourikis, Anastasios I and Roumeliotis, Stergios I}, - booktitle = {Proceedings 2007 IEEE International Conference on Robotics and Automation}, - pages = {3565--3572}, - year = {2007}, - organization = {IEEE}, - url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf}, -} - + title = {A multi-state constraint Kalman filter for vision-aided inertial navigation}, + author = {Mourikis, Anastasios I and Roumeliotis, Stergios I}, + year = 2007, + booktitle = {Proceedings 2007 IEEE International Conference on Robotics and Automation}, + pages = {3565--3572}, + url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf}, + organization = {IEEE} +} @phdthesis{Li2014THESIS, - title = {Visual-inertial odometry on resource-constrained systems}, - author = {Li, Mingyang}, - year = {2014}, - school = {UC Riverside}, - url = {https://escholarship.org/uc/item/4nn0j264}, + title = {Visual-inertial odometry on resource-constrained systems}, + author = {Li, Mingyang}, + year = 2014, + url = {https://escholarship.org/uc/item/4nn0j264}, + school = {UC Riverside} } - @inproceedings{Zhang2018IROS, - title = {A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry}, - author = {Zhang, Zichao and Scaramuzza, Davide}, - booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, - pages = {7244--7251}, - year = {2018}, - organization = {IEEE}, - url = {http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf} -} - - + title = {A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry}, + author = {Zhang, Zichao and Scaramuzza, Davide}, + year = 2018, + booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pages = {7244--7251}, + url = {http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf}, + organization = {IEEE} +} @inproceedings{Delmerico2018ICRA, - title = {A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots}, - author = {Delmerico, Jeffrey and Scaramuzza, Davide}, - booktitle = {2018 IEEE International Conference on Robotics and Automation (ICRA)}, - pages = {2502--2509}, - year = {2018}, - organization = {IEEE}, - url = {http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf}, -} - + title = {A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots}, + author = {Delmerico, Jeffrey and Scaramuzza, Davide}, + year = 2018, + booktitle = {2018 IEEE International Conference on Robotics and Automation (ICRA)}, + pages = {2502--2509}, + url = {http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf}, + organization = {IEEE} +} @inproceedings{Delmerico2019ICRA, - title={Are we ready for autonomous drone racing? the UZH-FPV drone racing dataset}, - author={Delmerico, Jeffrey and Cieslewski, Titus and Rebecq, Henri and Faessler, Matthias and Scaramuzza, Davide}, - booktitle={2019 International Conference on Robotics and Automation (ICRA)}, - pages={6713--6719}, - year={2019}, - organization={IEEE}, - url = {http://rpg.ifi.uzh.ch/docs/ICRA19_Delmerico.pdf}, -} - + title = {Are we ready for autonomous drone racing? the UZH-FPV drone racing dataset}, + author = {Delmerico, Jeffrey and Cieslewski, Titus and Rebecq, Henri and Faessler, Matthias and Scaramuzza, Davide}, + year = 2019, + booktitle = {2019 International Conference on Robotics and Automation (ICRA)}, + pages = {6713--6719}, + url = {http://rpg.ifi.uzh.ch/docs/ICRA19_Delmerico.pdf}, + organization = {IEEE} +} @article{Eckenhoff2019IJRR, - author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang}, - title = {Closed-form preintegration methods for graph-based visual-inertial navigation}, - journal = {International Journal of Robotics Research}, - volume = {38}, - number = {5}, - year = {2019}, - doi = {10.1177/0278364919835021}, - url = {https://doi.org/10.1177/0278364919835021} -} - + title = {Closed-form preintegration methods for graph-based visual-inertial navigation}, + author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang}, + year = 2019, + journal = {International Journal of Robotics Research}, + volume = 38, + number = 5, + doi = {10.1177/0278364919835021}, + url = {https://doi.org/10.1177/0278364919835021} +} @article{Qin2018TRO, - title={Vins-mono: A robust and versatile monocular visual-inertial state estimator}, - author={Qin, Tong and Li, Peiliang and Shen, Shaojie}, - journal={IEEE Transactions on Robotics}, - volume={34}, - number={4}, - pages={1004--1020}, - year={2018}, - publisher={IEEE}, - url = {https://arxiv.org/pdf/1708.03852.pdf}, -} - + title = {Vins-mono: A robust and versatile monocular visual-inertial state estimator}, + author = {Qin, Tong and Li, Peiliang and Shen, Shaojie}, + year = 2018, + journal = {IEEE Transactions on Robotics}, + publisher = {IEEE}, + volume = 34, + number = 4, + pages = {1004--1020}, + url = {https://arxiv.org/pdf/1708.03852.pdf} +} @article{Jeong2019IJRR, - title={Complex urban dataset with multi-level sensors from highly diverse urban environments}, - author={Jeong, Jinyong and Cho, Younggun and Shin, Young-Sik and Roh, Hyunchul and Kim, Ayoung}, - journal={The International Journal of Robotics Research}, - volume={38}, - number={6}, - pages={642--657}, - year={2019}, - publisher={SAGE Publications Sage UK: London, England} -} - - - - - + title = {Complex urban dataset with multi-level sensors from highly diverse urban environments}, + author = {Jeong, Jinyong and Cho, Younggun and Shin, Young-Sik and Roh, Hyunchul and Kim, Ayoung}, + year = 2019, + journal = {The International Journal of Robotics Research}, + publisher = {SAGE Publications Sage UK: London, England}, + volume = 38, + number = 6, + pages = {642--657} +} @inproceedings{Wagstaff2017IPIN, - title={Improving foot-mounted inertial navigation through real-time motion classification}, - author={Wagstaff, Brandon and Peretroukhin, Valentin and Kelly, Jonathan}, - booktitle={2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN)}, - pages={1--8}, - year={2017}, - organization={IEEE}, - url = {https://arxiv.org/pdf/1707.01152.pdf}, -} - + title = {Improving foot-mounted inertial navigation through real-time motion classification}, + author = {Wagstaff, Brandon and Peretroukhin, Valentin and Kelly, Jonathan}, + year = 2017, + booktitle = {2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN)}, + pages = {1--8}, + url = {https://arxiv.org/pdf/1707.01152.pdf}, + organization = {IEEE} +} @article{Ramanandan2011TITS, - title={Inertial navigation aiding by stationary updates}, - author={Ramanandan, Arvind and Chen, Anning and Farrell, Jay A}, - journal={IEEE Transactions on Intelligent Transportation Systems}, - volume={13}, - number={1}, - pages={235--248}, - year={2011}, - publisher={IEEE}, -} - + title = {Inertial navigation aiding by stationary updates}, + author = {Ramanandan, Arvind and Chen, Anning and Farrell, Jay A}, + year = 2011, + journal = {IEEE Transactions on Intelligent Transportation Systems}, + publisher = {IEEE}, + volume = 13, + number = 1, + pages = {235--248} +} @inproceedings{Davidson2009ENC, - title={Improved vehicle positioning in urban environment through integration of GPS and low-cost inertial sensors}, - author={Davidson, Pavel and Hautam{\"a}ki, Jani and Collin, Jussi and Takala, Jarmo}, - booktitle={Proceedings of the European Navigation Conference (ENC), Naples, Italy}, - pages={3--6}, - year={2009}, - url = {http://www.tkt.cs.tut.fi/research/nappo_files/1_C2.pdf}, -} - + title = {Improved vehicle positioning in urban environment through integration of GPS and low-cost inertial sensors}, + author = {Davidson, Pavel and Hautam{\"a}ki, Jani and Collin, Jussi and Takala, Jarmo}, + year = 2009, + booktitle = {Proceedings of the European Navigation Conference (ENC), Naples, Italy}, + pages = {3--6}, + url = {http://www.tkt.cs.tut.fi/research/nappo_files/1_C2.pdf} +} @article{Jeon2021RAL, - title={Run your visual-inertial odometry on NVIDIA Jetson: Benchmark tests on a micro aerial vehicle}, - author={Jeon, Jinwoo and Jung, Sungwook and Lee, Eungchang and Choi, Duckyu and Myung, Hyun}, - journal={IEEE Robotics and Automation Letters}, - volume={6}, - number={3}, - pages={5332--5339}, - year={2021}, - publisher={IEEE} -} - + title = {Run your visual-inertial odometry on NVIDIA Jetson: Benchmark tests on a micro aerial vehicle}, + author = {Jeon, Jinwoo and Jung, Sungwook and Lee, Eungchang and Choi, Duckyu and Myung, Hyun}, + year = 2021, + journal = {IEEE Robotics and Automation Letters}, + publisher = {IEEE}, + volume = 6, + number = 3, + pages = {5332--5339} +} @inproceedings{Dong2012IROS, - title={Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration}, - author={Dong-Si, Tue-Cuong and Mourikis, Anastasios I}, - booktitle={2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, - pages={1064--1071}, - year={2012}, - organization={IEEE} -} - - - + title = {Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration}, + author = {Dong-Si, Tue-Cuong and Mourikis, Anastasios I}, + year = 2012, + booktitle = {2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, + pages = {1064--1071}, + organization = {IEEE} +} +@inproceedings{Li2014ICRA, + title = {High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation}, + author = {Mingyang Li and Hongsheng Yu and Xing Zheng and and Anastasios I. Mourikis}, + year = 2014, + month = {May}, + booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, + pages = {409--416}, + url = {https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.696.3790&rep=rep1&type=pdf} +} +@article{Schneider2019Sensor, + title = {Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation}, + author = {Schneider, Thomas and Li, Mingyang and Cadena, Cesar and Nieto, Juan and Siegwart, Roland}, + year = 2019, + journal = {IEEE Sensors Journal}, + publisher = {IEEE}, + volume = 19, + number = 10, + pages = {3846--3860}, + url = {https://arxiv.org/abs/1901.07242} +} +@book{Chirikjian2011, + title = {Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications}, + author = {Chirikjian, Gregory}, + year = 2011, + publisher = {Springer Science \& Business Media}, + volume = 2 +} +@book{Barfoot2017, + title = {State estimation for robotics}, + author = {Barfoot, Timothy D}, + year = 2017, + publisher = {Cambridge University Press} +} +@techreport{Eckenhoff2018TR, + title = {Continuous Preintegration Theory for Visual-Inertial Navigation}, + author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang}, + year = 2018, + number = {RPNG-2018-CPI}, + note = {Available: \url{http://udel.edu/~ghuang/papers/tr_cpi.pdf}}, + institution = {University of Delaware} +} +@inproceedings{Yang2020ICRA, + title = {Analytic Combined IMU Integration (ACI$^2$) for Visual-Inertial Navigation}, + author = {Yulin Yang and B. P. W. Babu and Chuchu Chen and Guoquan Huang and Liu Ren}, + year = 2020, + booktitle = {Proc. of the IEEE International Conference on Robotics and Automation}, + address = {Paris, France} +} +@techreport{Yang2019TR_ACI, + title = {Supplementary Material: Analytic Combined IMU Integrator (ACI$^2$) For Visual Inertial Navigation}, + author = {Yulin Yang and Chuchu Chen and Guoquan Huang}, + year = 2019, + note = {Available: \url{http://udel.edu/~yuyang/downloads/suplementary\_2020ICRA.pdf}}, + institution = {RPNG, University of Delaware} +} +@article{Hesch2013TRO, + title = {Consistency analysis and improvement of vision-aided inertial navigation}, + author = {Hesch, Joel A and Kottas, Dimitrios G and Bowman, Sean L and Roumeliotis, Stergios I}, + year = 2013, + journal = {IEEE Transactions on Robotics}, + publisher = {IEEE}, + volume = 30, + number = 1, + pages = {158--176} +} diff --git a/docs/dev-roadmap.dox b/docs/dev-roadmap.dox index 2a89de383..e7afba8e8 100644 --- a/docs/dev-roadmap.dox +++ b/docs/dev-roadmap.dox @@ -3,20 +3,15 @@ @page dev-roadmap Future Roadmap - The initial release provides the library foundation which contains a filter-based visual-inertial localization solution. This can be used in a wide range of scenarios and the type-based index system allows for others to easily add new features and develop on top of this system. Here is a list of future directions, although nothing is set in stone, that we are interested in taking: - Creation of a secondary graph-based thread that loosely introduces loop closures (akin to the second thread of VINS-Mono and others) which should allow for drift free long-term localization. -- Large scale offline batch graph optimization which leverages the trajectory of the ov_msckf as its initial guess and then optimizes both the map and trajectory. +- Large scale offline batch graph optimization which leverages the trajectory of the ov_msckf as its initial guess and then optimizes both the map and trajectory (see [ov_maplab](https://github.com/rpng/ov_maplab) project!). - Incorporate our prior work in preintegration @cite Eckenhoff2019IJRR into the same framework structure to allow for easy extensibility. Focus on sparsification and marginalization to allow for realtime computation. - Leverage this sliding-window batch method to perform SFM initialization of all methods. - Support for arbitrary Schmidt'ing of state elements allowing for modeling of their prior uncertainties but without optimizing their values online. -- More advance imu integration schemes, quantification of the integration noises to handle low frequency readings, and modeling of the imu intrinsics. - - - */ \ No newline at end of file diff --git a/docs/propagation-analytical.dox b/docs/propagation-analytical.dox index f819c5ab8..211dbb302 100644 --- a/docs/propagation-analytical.dox +++ b/docs/propagation-analytical.dox @@ -3,6 +3,26 @@ @page propagation_analytical Analytical Propagation @tableofcontents +The state definition and general propagation equations are covered in the @ref propagation page. +Here we will cover the continuos-time integration of the state dynamics and inclusion of IMU intrinsic parameters. +First we will cover how we can propagate our mean forward, +afterwhich we cover how to derive the linearized error-state system propagation and approximations used. +Key references for analytical inertial integration include: + +- Indirect Kalman Filter for 3D Attitude Estimation @cite Trawny2005TR +- Consistency Analysis and Improvement of Vision-aided Inertial Navigation @cite Hesch2013TRO +- Closed-form preintegration methods for graph-based visual-inertial navigation @cite Eckenhoff2019IJRR @cite Eckenhoff2018TR +- Analytic Combined IMU Integration (ACI^2) for Visual-Inertial Navigation @cite Yang2020ICRA @cite Yang2019TR_ACI + + + +@section analytical_prop Analytical IMU Propagation + +Consider the following + + + + */ \ No newline at end of file diff --git a/docs/propagation-discrete.dox b/docs/propagation-discrete.dox index 3d0bf2f6f..56061d43f 100644 --- a/docs/propagation-discrete.dox +++ b/docs/propagation-discrete.dox @@ -3,85 +3,77 @@ @page propagation_discrete Discrete Propagation @tableofcontents +The state definition and general propagation equations are covered in the @ref propagation page. +Here we will cover a simplified discrete inertial model which does not include IMU intrinsic parameters. +First we will cover how we can propagate our mean forward, +afterwhich we cover how to derive the linearized discrete error-state system. -@section discrete_imu_measurements IMU Measurements -We use a 6-axis inertial measurement unit (IMU) to propagate the inertial navigation system (INS), -which provides measurements of the local rotational velocity (angular rate) \f$\boldsymbol{\omega}_m\f$ and -local translational acceleration \f$\mathbf{a}_m\f$: + +@section disc_quat_integration Zeroth Order Quaternion Integrator + +First we look at how to integrate our orientation equation forward in time. +One could use the integration property of a \f$SO(3)\f$'s matrix exponential or as we will do here, derive it for a quaternion orientation. +The solution to the quaternion evolution has the following general form +(see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR Section 1.6): \f{align*}{ - \boldsymbol{\omega}_m(t) &= \boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ - \mathbf{a}_m(t) &= \mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) + ^{I_{t}}_{G}\bar{q} = \boldsymbol{\Theta}(t,t_k) \text{}^{I_{k}}_{G}\bar{q} \f} -where \f$\boldsymbol{\omega}\f$ and \f$\mathbf{a}\f$ are the true rotational velocity and translational -acceleration in the IMU local frame \f$\{I\}\f$, -\f$\mathbf{b}_{g}\f$ and \f$\mathbf{b}_{a}\f$ are the gyroscope and accelerometer biases, and \f$\mathbf{n}_{{g}}\f$ -\f$\mathbf{n}_{{a}}\f$ are white Gaussian noise, -\f${^G\mathbf{g}} = [ 0 ~~ 0 ~~ 9.81 ]^\top\f$ is the gravity expressed in the global frame \f$\{G\}\f$ -(noting that the gravity is slightly different on different locations of the globe), -and \f$^I_G\mathbf{R}\f$ is the rotation matrix from global to IMU local frame. +Differentiating and reordering the terms yields the governing equation for +\f$\boldsymbol{\Theta}(t,t_k)\f$ as +\f{align*}{ + \boldsymbol{\Theta}(t,t_k) &= \text{}^{I_t}_G\bar{q} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ + \Rightarrow \dot{\boldsymbol{\Theta}}(t,t_k) &= + \text{}^{I_t}_G\dot{\bar{q}} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ + &= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \text{ }^{I_t}_{G}\bar{q} + \text{ }^{I_k}_{G}\bar{q}^{-1}\\ + &= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \boldsymbol{\Theta}(t,t_k) +\f} -@section discrete_ins_state State Vector - -We define our INS state vector \f$\mathbf{x}_I\f$ at time \f$t\f$ as: +where we have defined: \f{align*}{ -\mathbf{x}_I(t) = -\begin{bmatrix} -^I_G\bar{q}(t) \\ -^G\mathbf{p}_I(t) \\ -^G\mathbf{v}_I(t)\\ -\mathbf{b}_{\mathbf{g}}(t) \\ -\mathbf{b}_{\mathbf{a}}(t) -\end{bmatrix} +\boldsymbol{\Omega}(\boldsymbol{\omega}(t)) &= +\begin{bmatrix} -\lfloor \boldsymbol{\omega}(t) \times \rfloor +&& \boldsymbol{\omega}(t) \\ +-\boldsymbol{\omega}(t)^\top && 0 \end{bmatrix} \f} -where \f$^I_G\bar{q}\f$ is the unit quaternion representing the rotation global to IMU frame, -\f$^G\mathbf{p}_I\f$ is the position of IMU in global frame, -and \f$^G\mathbf{v}_I\f$ is the velocity of IMU in global frame. -We will often write time as a subscript of \f$I\f$ describing the state of IMU at the time -for notation clarity (e.g., \f$^{I_t}_G\bar{q} = \text{}^I_G\bar{q}(t)\f$). -In order to define the IMU error state, the standard additive error definition is employed -for the position, velocity, and biases, -while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quaternion multiplicative error -\f$\otimes\f$: +and \f$\boldsymbol{\Theta}(t_k,t_k) = \mathbf{I}_{4}\f$. +If we take \f$\boldsymbol{\omega}(t) = \boldsymbol{\omega}\f$ to be constant over the the period +\f$\Delta t_k = t_{k+1} - t_k\f$, +then the above system is linear time-invariant (LTI), and +\f$\boldsymbol{\Theta}\f$ can be solved as (see @cite Maybeck1982STOC): \f{align*}{ - ^I_G\bar{q} &= \delta \bar{q} \otimes \text{}^I_G \hat{\bar{q}}\\ - \delta \bar{q} &= \begin{bmatrix} - \hat{\mathbf{k}}\sin(\frac{1}{2}\tilde{\theta})\\ - \cos(\frac{1}{2}\tilde{\theta}) \end{bmatrix} - \simeq - \begin{bmatrix} - \frac{1}{2}\tilde{\boldsymbol{\theta}}\\ 1 - \end{bmatrix} + \boldsymbol{\Theta}(t_{k+1},t_k) + &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\Delta t_k\bigg)\\ + &= \cos\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t_k \bigg) \cdot \mathbf{I}_4 + + \frac{1}{|\boldsymbol{\omega}|}\sin\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t_k \bigg) + \cdot \boldsymbol{\Omega}(\boldsymbol{\omega})\\ +&\simeq + \mathbf{I}_4 + \frac{\Delta t_k}{2}\boldsymbol{\Omega}(\boldsymbol{\omega}) \f} -where \f$\hat{\mathbf{k}}\f$ is the rotation axis and \f$\tilde{\theta}\f$ is the rotation angle. -For small rotation, the error angle vector is approximated by \f$\tilde{\boldsymbol{\theta}} = \tilde{\theta}~\hat{\mathbf{k}}\f$ -as the error vector about the three orientation axes. -The total IMU error state thus is defined as the following 15x1 (not 16x1) vector: +where \f$\exp(\cdot)\f$ is the general matrix exponential which has a closed form solution, and +the approximation assumes small \f$|\boldsymbol{\omega}|\f$. +We can formulate the quaternion propagation from \f$t_k\f$ to \f$t_{k+1}\f$ using the estimated +rotational velocity \f$\hat{\boldsymbol{\omega}}(t) = \hat{\boldsymbol{\omega}}\f$ as: \f{align*}{ -\tilde{\mathbf{x}}_I(t) = -\begin{bmatrix} -^I_G\tilde{\boldsymbol{\theta}}(t) \\ -^G\tilde{\mathbf{p}}_I(t) \\ -^G\tilde{\mathbf{v}}_I(t)\\ -\tilde{\mathbf{b}}_{{g}}(t) \\ -\tilde{\mathbf{b}}_{{a}}(t) -\end{bmatrix} + \text{}^{I_{k+1}}_{G}\hat{\bar{q}} + = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})\Delta t_k\bigg) + \text{}^{I_{k}}_{G}\hat{\bar{q}} \f} @section disc_prop Discrete-time IMU Propagation - -A simpler method is to model the measurements as discrete-time over the integration period. +We model the measurements as discrete-time over the integration period. To do this, the measurements can be assumed to be constant during the sampling period. We employ this assumption and approximate that the measurement at time \f$t_k\f$ remains the same until we get the next measurement at \f$t_{k+1}\f$. @@ -92,89 +84,88 @@ Therefore the propagation of quaternion can be written as: \f{align*}{ \text{}^{I_{k+1}}_{G}\hat{\bar{q}} - = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) + = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-{\mathbf{b}}_{g,k} - \mathbf{n}_{g,k}\big)\Delta t_k\bigg) \text{}^{I_{k}}_{G}\hat{\bar{q}} \f} +where \f$\Delta t_k = t_{k+1} - t_{k}\f$. For the velocity and position propagation we have constant \f$\mathbf{a}_{m}(t_k) = \mathbf{a}_{m,k}\f$ over \f$t \in [t_k, t_{k+1}]\f$. We can therefore directly solve for the new states as: \f{align*}{ - ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t - +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ - - ^G\hat{\mathbf{p}}_{I_{k+1}} - &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 - + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 + ^G{\mathbf{p}}_{I_{k+1}} + &= \text{}^G{\mathbf{p}}_{I_k} + {}^G{\mathbf{v}}_{I_k} \Delta t_k + - \frac{1}{2}{}^G\mathbf{g}\Delta t_k^2 + + \frac{1}{2} \text{}^{I_k}_{G}{\mathbf{R}}^\top(\mathbf{a}_{m,k} - {\mathbf{b}}_{{a},k} - \mathbf{n}_{a,k})\Delta t_k^2 \\ + ^G{\mathbf{v}}_{k+1} &= \text{}^G{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t_k + +\text{}^{I_k}_{G}{\mathbf{R}}^\top(\mathbf{a}_{m,k} - {\mathbf{b}}_{{a},k} - \mathbf{n}_{a,k})\Delta t_k \f} The propagation of each bias is likewise the continuous system: \f{align*}{ - \hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k}\\ - \hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} + {\mathbf{b}}_{g,k+1} &= {\mathbf{b}}_{g,k} + \mathbf{n}_{wg,k} \\ + {\mathbf{b}}_{a,k+1} &= {\mathbf{b}}_{a,k} + \mathbf{n}_{wa,k} \f} - - - - - - -@section error_prop Discrete-time Error-state Propagation - -In order to propagate the covariance matrix, we should derive the error-state propagation, -i.e., computing the system Jacobian \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and noise Jacobian \f$\mathbf{G}_{k}\f$. -In particular, when the covariance matrix of the continuous-time measurement noises is given by -\f$\mathbf{Q}_c\f$, then the discrete-time noise covariance \f$\mathbf{Q}_d\f$ can be computed as +Note that the noises used here are the discrete ones which need to be converted from their continuous-time versions. +In particular, when the covariance matrix of the continuous-time measurement noises is given by +\f$\mathbf{Q}_c\f$ and \f$\mathbf{n}_c = [ \mathbf{n}_{g_c} ~ \mathbf{n}_{a_c} ~ \mathbf{n}_{wg_c} ~ \mathbf{n}_{wa_c} ]^\top\f$, +then the discrete-time noise covariance \f$\mathbf{Q}_d\f$ can be computed as (see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR Eq. (129) and (130)): \f{align*}{ -\sigma_{g} &= \frac{1}{\sqrt{\Delta t}}~ \sigma_{g_c} \\ -\sigma_{bg} &= \sqrt{\Delta t}~ \sigma_{bg_c} \\[1em] -\mathbf{Q}_{meas} &= +\sigma_{g} &= \frac{1}{\sqrt{\Delta t}}~ \sigma_{w_c} \\ +\sigma_{wg} &= \sqrt{\Delta t}~ \sigma_{wg_c} \\[1em] +\mathbf{Q}_{meas} &= \begin{bmatrix} \frac{1}{\Delta t}~ \sigma_{g_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \frac{1}{\Delta t}~ \sigma_{a_c}^2~ \mathbf{I}_3 \end{bmatrix} \\ -\mathbf{Q}_{bias} &= +\mathbf{Q}_{bias} &= \begin{bmatrix} -\Delta t~ \sigma_{bg_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ -\mathbf{0}_3 & \Delta t~ \sigma_{ba_c}^2~ \mathbf{I}_3 +\Delta t~ \sigma_{wg_c}^2~ \mathbf{I}_3 & \mathbf{0}_3 \\ +\mathbf{0}_3 & \Delta t~ \sigma_{wa_c}^2~ \mathbf{I}_3 \end{bmatrix} \f} -where \f$\mathbf{n} = [ \mathbf{n}_g ~ \mathbf{n}_a ~ \mathbf{n}_{bg} ~ \mathbf{n}_{ba} ]^\top\f$ are the discrete +where \f$\mathbf{n} = [ \mathbf{n}_{g} ~ \mathbf{n}_{a} ~ \mathbf{n}_{wg} ~ \mathbf{n}_{wa} ]^\top\f$ are the discrete IMU sensor noises which have been converted from their continuous representations. We define the stacked discrete measurement noise as follows: \f{align*}{ -\mathbf{Q}_{d} &= +\mathbf{Q}_{d} &= \begin{bmatrix} \mathbf{Q}_{meas} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{Q}_{bias} \end{bmatrix} \f} + + +@section error_prop Discrete-time Error-state Propagation + +In order to propagate the covariance matrix, we should derive the linearized error-state propagation, +i.e., computing the system Jacobian \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and noise Jacobian \f$\mathbf{G}_{k}\f$. The method of computing Jacobians is to "perturb" each variable in the system and see how the old error "perturbation" relates to the new error state. That is, \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ can be found by perturbing each variable as: \f{align*}{ \tilde{\mathbf{x}}_I(t_{k+1}) = \boldsymbol{\Phi}(t_{k+1},t_k) \tilde{\mathbf{x}}_I(t_{k}) + \mathbf{G}_{k} \mathbf{n} \f} -For the orientation error propagation, we start with the \f$\mathbf{SO}(3)\f$ perturbation using -\f${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}\f$: +For the orientation error propagation, we start with the \f${SO}(3)\f$ perturbation using +\f${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}\f$ +(which can be shown to be exactly equivalent to the state's quaternion error @cite Trawny2005TR): \f{align*}{ {}^{I_{k+1}}_G \mathbf{R} &= \text{}^{I_{k+1}}_{I_{k}} \mathbf{R} \text{}^{I_{k}}_G \mathbf{R} \\ (\mathbf{I}_3 - \lfloor ^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k+1}}_{G} \hat{\mathbf{R}} -&\approx \textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) +&\approx \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) (\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} \hat{\mathbf{R}}\\ -&=\textrm{exp}(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\textrm{exp}(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) +&=\exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\exp(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) (\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} \hat{\mathbf{R}}\\ &=\text{}^{I_{k+1}}_{I_{k}} \hat{\mathbf{R}} @@ -185,17 +176,17 @@ For the orientation error propagation, we start with the \f$\mathbf{SO}(3)\f$ pe \f} where \f$\tilde{\boldsymbol{\omega}} = \boldsymbol{\omega} - \hat{\boldsymbol{\omega}} -= -(\tilde{\mathbf{b}}_{\mathbf{g}} + \mathbf{n}_g)\f$ handles both the perturbation to the bias and measurement noise. -\f$\mathbf {J}_r(\boldsymbol{\theta})\f$ is the right Jacobian of \f$\mathbf{SO}(3)\f$ += -(\tilde{\mathbf{b}}_{{g}} + \mathbf{n}_g)\f$ handles both the perturbation to the bias and measurement noise. +\f$\mathbf {J}_r(\boldsymbol{\theta})\f$ is the right Jacobian of \f${SO}(3)\f$ that maps the variation of rotation angle in the parameter vector space into the variation in the tangent vector space to the manifold -[see @ref ov_core::Jr_so3()]. +[see @ref ov_core::Jr_so3() and @cite Barfoot2017]. By neglecting the second order terms from above, we obtain the following orientation error propagation: \f{align*} \text{}^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}} \approx \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} \text{}^{I_k}_{G}\tilde{\boldsymbol{\theta}} - \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t (\tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{\mathbf{g},k}) +\Delta t (\tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{{g},k}) \f} Now we can do error propagation of position and velocity using the same scheme: @@ -227,43 +218,43 @@ Now we can do error propagation of position and velocity using the same scheme: \f} where \f$\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}} - = - (\tilde{\mathbf{b}}_{\mathbf{a}} + \mathbf{n}_{\mathbf{a}})\f$. + = - (\tilde{\mathbf{b}}_{{a}} + \mathbf{n}_{{a}})\f$. By neglecting the second order error terms, we obtain the following position and velocity error propagation: \f{align*}{ \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &= \text{}^G\tilde{\mathbf{p}}_{I_k} -+ \Delta t \text{}^G\tilde{\mathbf{v}}_{I_k} ++ \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t - \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 -(\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k})\\ +(\tilde{\mathbf{b}}_{{a},k} + \mathbf{n}_{{a},k})\\ ^G\tilde{\mathbf{v}}_{k+1} &= \text{}^G\tilde{\mathbf{v}}_{I_k} - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t - (\tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{\mathbf{a},k}) + (\tilde{\mathbf{b}}_{{a},k} + \mathbf{n}_{{a},k}) \f} The propagation of the two random-walk biases are as follows: \f{align*}{ - \mathbf{b}_{\mathbf{g},k+1} &= \mathbf{b}_{\mathbf{g},k} + \mathbf{n}_{wg} \\ - \hat{\mathbf{b}}_{\mathbf{g},k+1} + \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= - \hat{\mathbf{b}}_{\mathbf{g},k} + \tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{b}_{\mathbf{g},k+1} &= \mathbf{b}_{{g},k} + \mathbf{n}_{wg} \\ + \hat{\mathbf{b}}_{{g},k+1} + \tilde{\mathbf{b}}_{{g},k+1} &= + \hat{\mathbf{b}}_{{g},k} + \tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{wg} \\ - \tilde{\mathbf{b}}_{\mathbf{g},k+1} &= - \tilde{\mathbf{b}}_{\mathbf{g},k} + \mathbf{n}_{wg} \\[1em] + \tilde{\mathbf{b}}_{{g},k+1} &= + \tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{wg} \\[1em] \mathbf{b}_{\mathbf{a},k+1} &= \mathbf{b}_{\mathbf{a},k} + \mathbf{n}_{wa} \\ - \hat{\mathbf{b}}_{\mathbf{a},k+1} + \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= - \hat{\mathbf{b}}_{\mathbf{a},k} + \tilde{\mathbf{b}}_{\mathbf{a},k} + \hat{\mathbf{b}}_{{a},k+1} + \tilde{\mathbf{b}}_{{a},k+1} &= + \hat{\mathbf{b}}_{{a},k} + \tilde{\mathbf{b}}_{{a},k} + \mathbf{n}_{wa} \\ - \tilde{\mathbf{b}}_{\mathbf{a},k+1} &= - \tilde{\mathbf{b}}_{\mathbf{a},k} + \mathbf{n}_{wa} + \tilde{\mathbf{b}}_{{a},k+1} &= + \tilde{\mathbf{b}}_{{a},k} + \mathbf{n}_{wa} \f} By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices as: diff --git a/docs/propagation.dox b/docs/propagation.dox index 59d7f94b6..67db0b368 100644 --- a/docs/propagation.dox +++ b/docs/propagation.dox @@ -11,11 +11,11 @@ which provides measurements of the local rotational velocity (angular rate) \f$\ local translational acceleration \f$\mathbf{a}_m\f$: \f{align*}{ - \boldsymbol{\omega}_m(t) &= \boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ - \mathbf{a}_m(t) &= \mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) + \boldsymbol{\omega}_m(t) &= {}^I\boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ + \mathbf{a}_m(t) &= {}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) \f} -where \f$\boldsymbol{\omega}\f$ and \f$\mathbf{a}\f$ are the true rotational velocity and translational +where \f${}^I\boldsymbol{\omega}\f$ and \f${}^I\mathbf{a}\f$ are the true rotational velocity and translational acceleration in the IMU local frame \f$\{I\}\f$, \f$\mathbf{b}_{g}\f$ and \f$\mathbf{b}_{a}\f$ are the gyroscope and accelerometer biases, and \f$\mathbf{n}_{{g}}\f$ \f$\mathbf{n}_{{a}}\f$ are white Gaussian noise, @@ -38,11 +38,12 @@ Thus we can define the following: where \f$\mathbf{T}_{w}\f$ and \f$\mathbf{T}_a\f$ are invertible \f$3\times3\f$ matrices which represent the scale imperfection and axis misalignment for \f$\{w\}\f$ and \f$\{a\}\f$, respectively. -\f${}^w_I\mathbf{R}\f$ and \f${}^a_I\mathbf{R}\f$ denote the rotation from the gyroscope frame and -acceleration frame to base "inertial" frame \f$\{I\}\f$ that can be determined to coincide with either. +\f${}^w_I\mathbf{R}\f$ and \f${}^a_I\mathbf{R}\f$ denote the rotation from the base "inertial" frame \f$\{I\}\f$ to +the gyroscope and acceleration sensor frames. \f$\mathbf{T}_g\f$ denotes the gravity sensitivity, which represents the effects of gravity to the gyroscope readings due to its inherent inertia. - +Note that this does not take into account the translation between the +gyroscope and accelerometer frames, since it is negligible for most IMUs (see @cite Li2014ICRA @cite Schneider2019Sensor). @@ -56,8 +57,8 @@ We define our INS state vector \f$\mathbf{x}_I\f$ at time \f$t\f$ as: ^I_G\bar{q}(t) \\ ^G\mathbf{p}_I(t) \\ ^G\mathbf{v}_I(t)\\ -\mathbf{b}_{\mathbf{g}}(t) \\ -\mathbf{b}_{\mathbf{a}}(t) +\mathbf{b}_{{g}}(t) \\ +\mathbf{b}_{{a}}(t) \end{bmatrix} \f} @@ -68,7 +69,7 @@ We will often write time as a subscript of \f$I\f$ describing the state of IMU a for notation clarity (e.g., \f$^{I_t}_G\bar{q} = \text{}^I_G\bar{q}(t)\f$). In order to define the IMU error state, the standard additive error definition is employed for the position, velocity, and biases, -while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quaternion multiplicative error +while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quaternion multiplicative error \f$\otimes\f$: \f{align*}{ @@ -102,157 +103,98 @@ The total IMU error state thus is defined as the following 15x1 (not 16x1) vecto -@section imu_kinematic IMU Kinematics +@section imu_kinematic IMU Kinematic Equations The IMU state evolves over time as follows (see [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf) @cite Trawny2005TR). - -\f{align*}{ \newcommand{\comm}[1]{} - +\f{align*}{ ^I_G\dot{\bar{q}}(t) - -\comm{ - &= \lim_{\delta t \to 0} \frac{1}{\delta t} - (^{I_{t + \delta t}}_G\bar{q} - \text{}^{I_{t}}_G\bar{q})\\ - - &= \lim_{\delta t \to 0} \frac{1}{\delta t} - (^{I_{t + \delta t}}_{L_{t}}\bar{q} \otimes ^{I_{t}}_{G}\bar{q} - - \bar{q}_0 \otimes \text{}^{I_{t}}_G\bar{q})\\ - - &= \lim_{\delta t \to 0} \frac{1}{\delta t} - (^{I_{t + \delta t}}_{L_{t}}\bar{q} - \bar{q}_0 ) \otimes \text{}^{I_{t}}_{G}\bar{q}\\ - - &\approx \lim_{\delta t \to 0} \frac{1}{\delta t} - \Bigg (\begin{bmatrix} \frac{1}{2}\delta \boldsymbol{\theta}\\ 1 \end{bmatrix} - -\begin{bmatrix} \boldsymbol{0}\\ 1 \end{bmatrix} \Bigg ) - \otimes \text{}^{I_{t}}_{G}\bar{q} \\ - - &= \frac{1}{2} \begin{bmatrix} \boldsymbol{\omega}\\ 0 \end{bmatrix} - \otimes \text{}^{I_{t}}_{G}\bar{q}\\} - - &= \frac{1}{2} \begin{bmatrix} -\lfloor \boldsymbol{\omega}(t) \times \rfloor - && \boldsymbol{\omega}(t) \\ - -\boldsymbol{\omega}^\top(t) && 0 \end{bmatrix} \text{}^{I_{t}}_{G}\bar{q}\\ - - &=: \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \text{}^{I_{t}}_{G}\bar{q}\\ - + &= \frac{1}{2} \begin{bmatrix} -\lfloor {}^{I}\boldsymbol{\omega}(t) \times \rfloor + && {}^{I}\boldsymbol{\omega}(t) \\ + -{}^{I}\boldsymbol{\omega}(t)^\top && 0 \end{bmatrix} \text{}^{I_{t}}_{G}\bar{q}\\ + &=: \frac{1}{2} \boldsymbol{\Omega}({}^{I}\boldsymbol{\omega}(t)) \text{}^{I_{t}}_{G}\bar{q}\\ ^G\dot{\mathbf{p}}_I(t) &=\text{} ^G\mathbf{v}_I(t)\\ - - ^G\dot{\mathbf{v}}_I(t) &=\text{} ^{I_{t}}_G\mathbf{R}^\top\mathbf{a}(t) \\ - - \dot{\mathbf{b}}_{\mathbf{g}}(t) &= \mathbf{n}_{wg}\\ - - \dot{\mathbf{b}}_{\mathbf{a}}(t) &= \mathbf{n}_{wa} + ^G\dot{\mathbf{v}}_I(t) &=\text{} ^{I_{t}}_G\mathbf{R}^\top {}^{I}\mathbf{a}(t) \\ + \dot{\mathbf{b}}_{\mathbf{g}}(t) &= \mathbf{n}_{wg}(t) \\ + \dot{\mathbf{b}}_{\mathbf{a}}(t) &= \mathbf{n}_{wa}(t) \f} -where we have modeled the gyroscope and accelerometer biases as random walk +where we have modeled the gyroscope and accelerometer biases as random walk and thus their time derivatives are white Gaussian. -Note that the above kinematics have been defined in terms of the *true* acceleration and angular velocities. - +Note that the above kinematics have been defined in terms of the *true* acceleration and +angular velocities which can be computed as a function of the sensor measurements and state. -@section conti_prop Continuous-time IMU Propagation +@section conti_prop Continuous-time Inertial Integration -Given the continuous-time measurements \f$\boldsymbol{\omega}_m(t)\f$ -and \f$\mathbf{a}_m(t)\f$ in the time interval \f$t \in [t_k,t_{k+1}]\f$, +Given the continuous-time \f${}^{I}\boldsymbol{\omega}(t)\f$ +and \f${}^{I}\mathbf{a}(t)\f$, in the time interval \f$t \in [t_k,t_{k+1}]\f$, and their estimates, i.e. after taking the expectation, -\f$\hat{\boldsymbol{\omega}}(t) = \boldsymbol{\omega}_m(t) - \hat{\boldsymbol{b}}_g(t)\f$ and -\f$\hat{\boldsymbol{a}}(t) = \boldsymbol{a}_m(t) - \hat{\boldsymbol{b}}_a(t) - ^I_G\hat{\mathbf{R}}(t){}^G\mathbf{g}\f$, +\f${}^{I}\hat{\boldsymbol{\omega}}(t)\f$ and \f${}^{I}\hat{\boldsymbol{a}}(t)\f$, we can define the solutions to the above IMU kinematics differential equation. -The solution to the quaternion evolution has the following general form: - -\f{align*}{ - ^{I_{t}}_{G}\bar{q} = \boldsymbol{\Theta}(t,t_k) \text{}^{I_{k}}_{G}\bar{q} -\f} - -Differentiating and reordering the terms yields the governing equation for -\f$\boldsymbol{\Theta}(t,t_k)\f$ as - -\f{align*}{ \newcommand{\comm}[1]{} - - \boldsymbol{\Theta}(t,t_k) &= \text{}^{I_t}_G\bar{q} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ - - \Rightarrow \dot{\boldsymbol{\Theta}}(t,t_k) &= - \text{}^{I_t}_G\dot{\bar{q}} \text{ }^{I_k}_{G}\bar{q}^{-1}\\ - &= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \text{ }^{I_t}_{G}\bar{q} - \text{ }^{I_k}_{G}\bar{q}^{-1}\\ - &= \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \boldsymbol{\Theta}(t,t_k) -\f} -with \f$\boldsymbol{\Theta}(t_k,t_k) = \mathbf{I}_{4}\f$. -If we take \f$\boldsymbol{\omega}(t) = \boldsymbol{\omega}\f$ to be constant over the the period -\f$\Delta t = t_{k+1} - t_k\f$, -then the above system is linear time-invarying (LTI), and -\f$\boldsymbol{\Theta}\f$ can be solved as (see [[Stochastic Models, Estimation, and Control]](https://books.google.com/books?id=L_YVMUJKNQUC) @cite Maybeck1982STOC): - - -\f{align*}{ - \boldsymbol{\Theta}(t_{k+1},t_k) - &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\Delta t\bigg)\\ - &= \cos\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t \bigg) \cdot \mathbf{I}_4 - + \frac{1}{|\boldsymbol{\omega}|}\sin\bigg(\frac{|\boldsymbol{\omega}|}{2} \Delta t \bigg) - \cdot \boldsymbol{\Omega}(\boldsymbol{\omega})\\ -&\simeq - \mathbf{I}_4 + \frac{\Delta t}{2}\boldsymbol{\Omega}(\boldsymbol{\omega}) -\f} - - -where the approximation assumes small \f$|\boldsymbol{\omega}|\f$. -We can formulate the quaternion propagation from \f$t_k\f$ to \f$t_{k+1}\f$ using the estimated -rotational velocity \f$\hat{\boldsymbol{\omega}}(t) = \hat{\boldsymbol{\omega}}\f$ as: - -\f{align*}{ - \text{}^{I_{k+1}}_{G}\hat{\bar{q}} - = \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})\Delta t\bigg) - \text{}^{I_{k}}_{G}\hat{\bar{q}} -\f} - -Having defined the integration of the orientation, we can integrate the velocity and position over the measurement interval: \f{align*}{ - ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} + \int_{t_{k}}^{t_{k+1}} - {^G\hat{\mathbf{a}}(\tau)} d\tau \\ - - &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t+ \int_{t_{k}}^{t_{k+1}} - {^G_{I_{\tau}}\hat{\mathbf{R}}(\mathbf{a}_m(\tau) - \hat{\mathbf{b}}_{\mathbf{a}}(\tau)) d\tau}\\[1em] - - ^G\hat{\mathbf{p}}_{I_{k+1}} &= - \text{}^G\hat{\mathbf{p}}_{I_k} + \int_{t_{k}}^{t_{k+1}} - {^G\hat{\mathbf{v}}_I(\tau)} d\tau \\ - - &= \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + - \int_{t_{k}}^{t_{k+1}} \int_{t_{k}}^{s} - {^G_{I_{\tau}}\hat{\mathbf{R}}(\mathbf{a}_m(\tau) - \hat{\mathbf{b}}_{\mathbf{a}}(\tau)) d\tau ds} + {}^{I_{k+1}}_{G}\mathbf{R} & = + \exp \left(-\int^{t_{k+1}}_{t_k} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau \right) {}^{I_k}_{G}\mathbf{R} \\ + &\triangleq \Delta \mathbf{R}_k {}^{I_k}_{G}\mathbf{R} \\ + {}^G\mathbf{p}_{I_{k+1}} & = + {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + + {}^{I_k}_{G}\mathbf{R}^\top + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau d s + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k \\ + &\triangleq + {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{p}_{k} + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k \\ + {}^G\mathbf{v}_{I_{k+1}} & = {}^G\mathbf{v}_{I_k} + + {}^{I_k}_{G}\mathbf{R}^\top + \int^{t_{k+1}}_{t_k} + {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau + - {}^G\mathbf{g}\Delta t_k \\ + &\triangleq {}^G\mathbf{v}_{I_k} + + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{v}_{k} + - {}^G\mathbf{g}\Delta t_k \\ + \mathbf{b}_{g_{k+1}} & = \mathbf{b}_{g_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wg}(t) ~ d \tau \\ + \mathbf{b}_{a_{k+1}} & = \mathbf{b}_{a_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wa}(t) ~ d \tau \f} -Propagation of each bias \f$\hat{\mathbf{b}}_{\mathbf{g}}\f$ and \f$\hat{\mathbf{b}}_{\mathbf{a}}\f$ is given by: +where \f$\Delta t_k = t_{k+1} - t_{k}\f$, +\f${}^{I_k}_{I_\tau}\mathbf{R} = \exp(\int^{t_\tau}_{t_k} {}^{I}\boldsymbol{\omega}(t_u) ~ d u ) \f$, +\f$\exp(\cdot)\f$ is the \f$SO(3)\f$ matrix exponential @cite Chirikjian2011, and vectors are evaluated at +their subscript timesteps (e.g. \f${}^G\mathbf{v}_{I_k}={}^G\mathbf{v}_{I}(t_k)\f$). +The biases are corrupted by random walk noises \f${\mathbf{n}}_{wg}\f$ and \f${\mathbf{n}}_{wa}\f$ that are zero-mean white Gaussians. +We have the following integration components: \f{align*}{ - \hat{\mathbf{b}}_{\mathbf{g},k+1} &= \hat{\mathbf{b}}_{\mathbf{g},k} + \int_{t_{k+1}}^{t_{k}} - {\hat{\mathbf{n}}_{wg}(\tau)} d\tau \\ - - &= \hat{\mathbf{b}}_{\mathbf{g},k} \\ - - \hat{\mathbf{b}}_{\mathbf{a},k+1} &= \hat{\mathbf{b}}_{\mathbf{a},k} + \int_{t_{k+1}}^{t_{k}} - {\hat{\mathbf{n}}_{wa}(\tau)} d\tau \\ - - &= \hat{\mathbf{b}}_{\mathbf{a},k} + \Delta \mathbf{R}_k & \triangleq + {}^{I_{k+1}}_{I_k}\mathbf{R} + = \exp + \left(-\int^{t_{k+1}}_{t_{k}} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau\right) + \\ + \Delta \mathbf{p}_{k} & \triangleq + \int^{t_{k+1}}_{t_{k}} \int^{s}_{t_{k}} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau d s + \\ + \Delta \mathbf{v}_{k} & \triangleq + \int^{t_{k+1}}_{t_{k}} + {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau \f} -The biases will not evolve since our random walk noises \f$\hat{\mathbf{n}}_{wg}\f$ and \f$\hat{\mathbf{n}}_{wa}\f$ are zero-mean white Gaussian. -All of the above integrals could be analytically or numerically solved if one wishes to use the continuous-time measurement evolution model. - - @section prop_integration Mean and Uncertainty Integration +Having defined the general equations for state evolution, we can now look into how to actually perform these integrations. +The following two documents detail two different ways to both evolve the state forward and its covariance. +The first is a simple discrete state evolution model without any IMU intrinsics and can be helpful to understand the general flow of derivation. +The second is the full continuous-time integration (with constant measurement assumption) of the mean and state covariance. - @subpage propagation_discrete --- Simplified discrete mean and covariance propagation derivations -- @subpage propagation_analytical --- Analytical mean and covariance propagation with IMU intriniscs derivations +- @subpage propagation_analytical --- Analytical mean and covariance propagation with IMU intrinsics derivations + + diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index 70c25b148..e0f57215c 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -492,10 +492,10 @@ inline Eigen::Matrix quatnorm(Eigen::Matrix q_t) { * @brief Computes left Jacobian of SO(3) * * The left Jacobian of SO(3) is defined equation (7.77b) in [State Estimation for - * Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) by Timothy D. Barfoot. Specifically it is the following (with - * \f$\theta=|\boldsymbol\theta|\f$ and \f$\mathbf a=\boldsymbol\theta/|\boldsymbol\theta|\f$): \f{align*}{ J_l(\boldsymbol\theta) = - * \frac{\sin\theta}{\theta}\mathbf I + \Big(1-\frac{\sin\theta}{\theta}\Big)\mathbf a \mathbf a^\top + \frac{1-\cos\theta}{\theta}\lfloor - * \mathbf a \times\rfloor \f} + * Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) by Timothy D. Barfoot @cite Barfoot2017. Specifically it is the + * following (with \f$\theta=|\boldsymbol\theta|\f$ and \f$\mathbf a=\boldsymbol\theta/|\boldsymbol\theta|\f$): \f{align*}{ + * J_l(\boldsymbol\theta) = \frac{\sin\theta}{\theta}\mathbf I + \Big(1-\frac{\sin\theta}{\theta}\Big)\mathbf a \mathbf a^\top + + * \frac{1-\cos\theta}{\theta}\lfloor \mathbf a \times\rfloor \f} * * @param w axis-angle * @return The left Jacobian of SO(3) @@ -516,8 +516,8 @@ inline Eigen::Matrix Jl_so3(const Eigen::Matrix &w) * @brief Computes right Jacobian of SO(3) * * The right Jacobian of SO(3) is related to the left by Jl(-w)=Jr(w). - * See equation (7.87) in [State Estimation for Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) by Timothy D. Barfoot. - * See @ref Jl_so3() for the definition of the left Jacobian of SO(3). + * See equation (7.87) in [State Estimation for Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) by Timothy D. Barfoot + * @cite Barfoot2017. See @ref Jl_so3() for the definition of the left Jacobian of SO(3). * * @param w axis-angle * @return The right Jacobian of SO(3) From 084835fb3162c58edc1b9de41c29ad42c19eb47e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 11 Jan 2022 17:55:10 -0500 Subject: [PATCH 37/60] more prop doc additions --- Doxyfile | 2 +- docs/bib/extra.bib | 17 ++++++ docs/propagation-analytical.dox | 105 +++++++++++++++++++++++++++++++- docs/propagation.dox | 11 ++-- 4 files changed, 128 insertions(+), 7 deletions(-) diff --git a/Doxyfile b/Doxyfile index fd27bd165..3734dd21d 100644 --- a/Doxyfile +++ b/Doxyfile @@ -843,7 +843,7 @@ EXCLUDE_SYMLINKS = NO # Note that the wildcards are matched against the file with absolute path, so to # exclude all test directories for example use the pattern */test/* -EXCLUDE_PATTERNS = */doxygen_generated/* */catkin_generated/* */cmake-build-debug/* */plot/* .github .idea +EXCLUDE_PATTERNS = */doxygen_generated/* */catkin_generated/* */cmake-build-debug/* */plot/* */ov_data/* .github .idea # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names # (namespaces, classes, functions, etc.) that should be excluded from the diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index bc946e582..fb731a2d9 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -272,3 +272,20 @@ @article{Hesch2013TRO number = 1, pages = {158--176} } +@article{Yang2020RSS, + title = {Online imu intrinsic calibration: Is it necessary?}, + author = {Yang, Yulin and Geneva, Patrick and Zuo, Xingxing and Huang, Guoquan}, + year = 2020, + journal = {Proc. of Robotics: Science and Systems (RSS), Corvallis, Or}, + url = {http://www.roboticsproceedings.org/rss16/p026.pdf} +} +@article{Rehder2017Sensors, + title = {Camera/IMU calibration revisited}, + author = {Rehder, Joern and Siegwart, Roland}, + year = 2017, + journal = {IEEE Sensors Journal}, + publisher = {IEEE}, + volume = 17, + number = 11, + pages = {3257--3268} +} diff --git a/docs/propagation-analytical.dox b/docs/propagation-analytical.dox index 211dbb302..6e54db1b7 100644 --- a/docs/propagation-analytical.dox +++ b/docs/propagation-analytical.dox @@ -15,8 +15,111 @@ Key references for analytical inertial integration include: - Analytic Combined IMU Integration (ACI^2) for Visual-Inertial Navigation @cite Yang2020ICRA @cite Yang2019TR_ACI +@section imu_intrinsic_models IMU Intrinsic Models -@section analytical_prop Analytical IMU Propagation +First we can re-write our IMU measurement models to find the the true (or corrected) +angular velocity \f${}^I\boldsymbol{\omega}(t)\f$ and linear acceleration \f${}^I\mathbf{a}(t)\f$. +This is how we will relate incoming measurements to IMU dynamics (see @ref imu_kinematic). + +\f{align*}{ +{}^I\boldsymbol{\omega}(t) + & = {}^I_{w}\mathbf{R} \mathbf{D}_w + \left( + {}^w\boldsymbol{\omega}_{m}(t) - \mathbf{T}_{g} {}^I\mathbf{a}(t) - \mathbf{b}_g(t) - \mathbf{n}_g(t) + \right) \\ +{}^I\mathbf{a}(t) + & = {}^I_a\mathbf{R} \mathbf{D}_a + \left( + {}^a\mathbf{a}_{m}(t) - \mathbf{b}_a(t) - \mathbf{n}_a(t) + \right) + - {}^I_G\mathbf{R}(t) {^G\mathbf{g}} +\f} + + +where \f$\mathbf{D}_w = \mathbf{T}^{-1}_w\f$ and \f$\mathbf{D}_a = \mathbf{T}^{-1}_a\f$. +In practice, we calibrate \f$\mathbf{D}_a\f$, \f$\mathbf{D}_w\f$, \f${}^I_a\mathbf{R}\f$ (or \f${}^I_w\mathbf{R}\f$) and \f$\mathbf{T}_g\f$ +to prevent the need to have an unneeded matrix inversion / transposes in the measurement equation. + +We only calibrate either \f${}^I_w\mathbf{R}\f$ or \f${}^I_a\mathbf{R}\f$ since the base ``inertial'' +frame must coincide with one frame arbitrarily (would otherwise introduce too many degrees of freedom). +If both \f${}^I_w\mathbf{R}\f$ and \f${}^I_a\mathbf{R}\f$ are calibrated, +it will make the rotation between the IMU and camera unobservable due to over parameterization @cite Yang2020RSS. +We define two different models of interested: + +- *KALIBR*: Contains \f$\mathbf{D}'_{w6}\f$, \f$\mathbf{D}'_{a6}\f$, \f${}^I_w\mathbf{R}\f$ and gravity sensitivity \f$\mathbf{T}_{g9}\f$. + This model follows IMU intrinsic calibration presented in @cite Rehder2017Sensors and the + output used in the open-source calibration toolbox Kalibr @cite Furgale2013IROS. + +- *RPNG*: Contains \f$\mathbf{D}_{w6}\f$, \f$\mathbf{D}_{a6}\f$, the rotation \f${}^I_a\mathbf{R}\f$, and gravity sensitivity \f$\mathbf{T}_{g9}\f$. + \f$\mathbf{D}_{a6}\f$ and \f$\mathbf{D}_{w6}\f$ are uptriangular matrices, and follows *imu2* analysed in @cite Yang2020RSS. + +where we can define \f$\mathbf{D}_{*6} = {\mathbf{D}'_{*6}}^\top\f$ and the following matrices: + +\f{align*}{ +\mathbf{D}_{*6} &= \begin{bmatrix} + d_{*1} & d_{*2} & d_{*4} \\ + 0 & d_{*3} & d_{*5} \\ + 0 & 0 & d_{*6} + \end{bmatrix} \\ +\mathbf{T}_{g9} &= \begin{bmatrix} + t_{g1} & t_{g4} & t_{g7} \\ + t_{g2} & t_{g5} & t_{g8} \\ + t_{g3} & t_{g6} & t_{g9} + \end{bmatrix} +\f} + + + + +@section analytical_state Full State with Calibration + +In the following derivations, we will compute the Jacobians for all the variables that might appear +in the IMU models, including scale/axis correction for gyroscope \f$\mathbf{D}_w\f$ (6 parameters), +scale/axis correction for accelerometer \f$\mathbf{D}_a\f$ (6 parameters), rotation from gyroscope to +IMU frame \f${}^I_w\mathbf{R}\f$, rotation from accelerometer to IMU frame \f${}^I_a\mathbf{R}\f$ and +gravity sensitivity \f$\mathbf{T}_g\f$ (9 parameters). +The state vector \f$\mathbf{x}\f$ contains the IMU state \f$\mathbf{x}_{I}\f$ and its intrinsics +\f$\mathbf{x}_{in}\f$: + +\f{align*}{ +\mathbf{x}_I & = + \begin{bmatrix} + \mathbf{x}^{\top}_n & | & \mathbf{x}^{\top}_b & | & \mathbf{x}^{\top}_{in} + \end{bmatrix}^{\top} + \\ +& = \begin{bmatrix} + {}^I_G\bar{q}^{\top} & + {}^G\mathbf{p}^{\top}_{I} & + {}^G\mathbf{v}^{\top}_{I} & + | & + \mathbf{b}^{\top}_{g} & + \mathbf{b}^{\top}_{a} & + | & \mathbf{x}^{\top}_{in} + \end{bmatrix}^{\top} \\ +\mathbf{x}_{in} & = + \begin{bmatrix} + \mathbf{x}^{\top}_{Dw} & \mathbf{x}^{\top}_{Da} & {}^I_w\bar{q}^{\top} & {}^I_a\bar{q}^{\top} & \mathbf{x}^{\top}_{Tg} + \end{bmatrix}^{\top} +\f} + +where \f${}^I_G\bar{q}\f$ denotes the rotation matrix \f${}^I_G\mathbf{R}\f$ from \f$\{G\}\f$ to \f$\{I\}\f$. +\f${}^G\mathbf{p}_{I}\f$ and \f${}^G\mathbf{v}_I\f$ denote the IMU position and velocity in \f$\{G\}\f$. +\f$\mathbf{x}_n\f$ denotes the IMU navigation states and \f$\mathbf{x}_b\f$ denotes the IMU biases. +IMU intrinsics, \f$\mathbf{x}_{in}\f$, contains the non-zero elements stored column-wise: + +\f{align*}{ +\mathbf{x}_{D*} & = + \begin{bmatrix} + d_{*1} & d_{*2} & d_{*3} & d_{*4} & d_{*5} & d_{*6} + \end{bmatrix}^{\top} \\ +\mathbf{x}_{Tg} & = + \begin{bmatrix} + t_{g1} & t_{g2} & t_{g3} & t_{g4} & t_{g5} & t_{g6} & t_{g7} & t_{g8} & t_{g9} + \end{bmatrix}^{\top} +\f} + + +@section analytical_prop Analytical State Integration Consider the following diff --git a/docs/propagation.dox b/docs/propagation.dox index 67db0b368..07594bfcb 100644 --- a/docs/propagation.dox +++ b/docs/propagation.dox @@ -11,8 +11,8 @@ which provides measurements of the local rotational velocity (angular rate) \f$\ local translational acceleration \f$\mathbf{a}_m\f$: \f{align*}{ - \boldsymbol{\omega}_m(t) &= {}^I\boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ - \mathbf{a}_m(t) &= {}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) + {}^I\boldsymbol{\omega}_m(t) &= {}^I\boldsymbol{\omega}(t) + \mathbf{b}_{g}(t) + \mathbf{n}_{{g}}(t)\\ + {}^I\mathbf{a}_m(t) &= {}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_{a}(t) + \mathbf{n}_{{a}}(t) \f} where \f${}^I\boldsymbol{\omega}\f$ and \f${}^I\mathbf{a}\f$ are the true rotational velocity and translational @@ -32,8 +32,9 @@ Thus we can define the following: {}^w\boldsymbol{\omega}_{m}(t) & = \mathbf{T}_{w} {}^w_I\mathbf{R} {}^I\boldsymbol{\omega}(t) + \mathbf{T}_{g} {}^I\mathbf{a}(t) + \mathbf{b}_g(t) + \mathbf{n}_g(t) \\ -{}^a\mathbf{a}_{m}(t) & = \mathbf{T}_a {}^a_I\mathbf{R} {}^I\mathbf{a}(t) - + {}^I_G\mathbf{R}(t) {^G\mathbf{g}} + \mathbf{b}_a(t) + \mathbf{n}_a(t) +{}^a\mathbf{a}_{m}(t) & = \mathbf{T}_a {}^a_I\mathbf{R} + ({}^I\mathbf{a}(t) + {}^I_G\mathbf{R}(t) {^G\mathbf{g}}) + + \mathbf{b}_a(t) + \mathbf{n}_a(t) \f} where \f$\mathbf{T}_{w}\f$ and \f$\mathbf{T}_a\f$ are invertible \f$3\times3\f$ matrices which represent the @@ -113,7 +114,7 @@ The IMU state evolves over time as follows &= \frac{1}{2} \begin{bmatrix} -\lfloor {}^{I}\boldsymbol{\omega}(t) \times \rfloor && {}^{I}\boldsymbol{\omega}(t) \\ -{}^{I}\boldsymbol{\omega}(t)^\top && 0 \end{bmatrix} \text{}^{I_{t}}_{G}\bar{q}\\ - &=: \frac{1}{2} \boldsymbol{\Omega}({}^{I}\boldsymbol{\omega}(t)) \text{}^{I_{t}}_{G}\bar{q}\\ + &\triangleq \frac{1}{2} \boldsymbol{\Omega}({}^{I}\boldsymbol{\omega}(t)) \text{}^{I_{t}}_{G}\bar{q}\\ ^G\dot{\mathbf{p}}_I(t) &=\text{} ^G\mathbf{v}_I(t)\\ ^G\dot{\mathbf{v}}_I(t) &=\text{} ^{I_{t}}_G\mathbf{R}^\top {}^{I}\mathbf{a}(t) \\ \dot{\mathbf{b}}_{\mathbf{g}}(t) &= \mathbf{n}_{wg}(t) \\ From 4599bd7b97ea373ee57f07eb9602f041afe7e8ea Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 13 Jan 2022 10:33:32 -0500 Subject: [PATCH 38/60] finish analytical docs, switch DeltaR_k and Jr_ktok1 to transposed direction to match discrete docs --- config/rpng_sim/estimator_config.yaml | 2 +- docs/bib/extra.bib | 49 +- docs/propagation-analytical.dox | 828 +++++++++++++++++++++++++- docs/propagation.dox | 10 +- ov_msckf/launch/simulation.launch | 3 +- ov_msckf/src/state/Propagator.cpp | 72 +-- ov_msckf/src/state/Propagator.h | 70 +-- 7 files changed, 918 insertions(+), 116 deletions(-) diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index dbcfcff50..eb6c72573 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -3,7 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -integration: "analytical" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index fb731a2d9..ead2c1f6a 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -6,7 +6,8 @@ @article{Patron2015IJCV publisher = {Springer}, volume = 113, number = 3, - pages = {208--219} + pages = {208--219}, + url = {https://link.springer.com/article/10.1007/s11263-015-0811-3} } @article{Mueggler2018TRO, title = {Continuous-Time Visual-Inertial Odometry for Event Cameras}, @@ -14,7 +15,7 @@ @article{Mueggler2018TRO year = 2018, journal = {IEEE Transactions on Robotics}, pages = {1--16}, - url = {http://rpg.ifi.uzh.ch/docs/TRO18_Mueggler.pdf} + url = {http://rpg.ifi.uzh.ch/docs/TRO18\%5FMueggler.pdf} } @article{Burri2016IJRR, title = {The EuRoC micro aerial vehicle datasets}, @@ -24,7 +25,8 @@ @article{Burri2016IJRR publisher = {SAGE Publications Sage UK: London, England}, volume = 35, number = 10, - pages = {1157--1163} + pages = {1157--1163}, + url = {https://journals.sagepub.com/doi/full/10.1177/0278364915620033} } @inproceedings{Schubert2018IROS, title = {The TUM VI benchmark for evaluating visual-inertial odometry}, @@ -41,6 +43,7 @@ @inproceedings{Furgale2013IROS year = 2013, booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems}, pages = {1280--1286}, + url = {https://ieeexplore.ieee.org/abstract/document/6696514}, organization = {IEEE} } @article{Trawny2005TR, @@ -58,7 +61,7 @@ @book{Maybeck1982STOC year = 1982, publisher = {Academic press}, volume = 3, - url = {https://books.google.com/books?id=L_YVMUJKNQUC} + url = {https://books.google.com/books?id=L\%5FYVMUJKNQUC} } @article{Huang2010IJRR, title = {Observability-based rules for designing consistent EKF SLAM estimators}, @@ -79,7 +82,8 @@ @article{Li2013IJRR publisher = {SAGE Publications Sage UK: London, England}, volume = 32, number = 6, - pages = {690--711} + pages = {690--711}, + url = {https://journals.sagepub.com/doi/full/10.1177/0278364913481251} } @book{Kay1993, title = {Fundamentals of statistical signal processing}, @@ -110,7 +114,7 @@ @inproceedings{Zhang2018IROS year = 2018, booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, pages = {7244--7251}, - url = {http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf}, + url = {http://rpg.ifi.uzh.ch/docs/IROS18\%5FZhang.pdf}, organization = {IEEE} } @inproceedings{Delmerico2018ICRA, @@ -119,7 +123,7 @@ @inproceedings{Delmerico2018ICRA year = 2018, booktitle = {2018 IEEE International Conference on Robotics and Automation (ICRA)}, pages = {2502--2509}, - url = {http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf}, + url = {http://rpg.ifi.uzh.ch/docs/ICRA18\%5FDelmerico.pdf}, organization = {IEEE} } @inproceedings{Delmerico2019ICRA, @@ -128,7 +132,7 @@ @inproceedings{Delmerico2019ICRA year = 2019, booktitle = {2019 International Conference on Robotics and Automation (ICRA)}, pages = {6713--6719}, - url = {http://rpg.ifi.uzh.ch/docs/ICRA19_Delmerico.pdf}, + url = {http://rpg.ifi.uzh.ch/docs/ICRA19\%5FDelmerico.pdf}, organization = {IEEE} } @article{Eckenhoff2019IJRR, @@ -160,7 +164,8 @@ @article{Jeong2019IJRR publisher = {SAGE Publications Sage UK: London, England}, volume = 38, number = 6, - pages = {642--657} + pages = {642--657}, + url = {https://journals.sagepub.com/doi/full/10.1177/0278364919843996} } @inproceedings{Wagstaff2017IPIN, title = {Improving foot-mounted inertial navigation through real-time motion classification}, @@ -179,7 +184,8 @@ @article{Ramanandan2011TITS publisher = {IEEE}, volume = 13, number = 1, - pages = {235--248} + pages = {235--248}, + url = {https://ieeexplore.ieee.org/abstract/document/6060912} } @inproceedings{Davidson2009ENC, title = {Improved vehicle positioning in urban environment through integration of GPS and low-cost inertial sensors}, @@ -187,7 +193,7 @@ @inproceedings{Davidson2009ENC year = 2009, booktitle = {Proceedings of the European Navigation Conference (ENC), Naples, Italy}, pages = {3--6}, - url = {http://www.tkt.cs.tut.fi/research/nappo_files/1_C2.pdf} + url = {http://www.tkt.cs.tut.fi/research/nappo\%5Ffiles/1\%5FC2.pdf} } @article{Jeon2021RAL, title = {Run your visual-inertial odometry on NVIDIA Jetson: Benchmark tests on a micro aerial vehicle}, @@ -197,7 +203,8 @@ @article{Jeon2021RAL publisher = {IEEE}, volume = 6, number = 3, - pages = {5332--5339} + pages = {5332--5339}, + url = {https://ieeexplore.ieee.org/abstract/document/9416140} } @inproceedings{Dong2012IROS, title = {Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration}, @@ -205,6 +212,7 @@ @inproceedings{Dong2012IROS year = 2012, booktitle = {2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, pages = {1064--1071}, + url = {https://intra.ece.ucr.edu/~mourikis/papers/DongSi2012IROS.pdf}, organization = {IEEE} } @inproceedings{Li2014ICRA, @@ -238,7 +246,8 @@ @book{Barfoot2017 title = {State estimation for robotics}, author = {Barfoot, Timothy D}, year = 2017, - publisher = {Cambridge University Press} + publisher = {Cambridge University Press}, + url = {http://asrl.utias.utoronto.ca/~tdb/bib/barfoot\%5Fser17.pdf} } @techreport{Eckenhoff2018TR, title = {Continuous Preintegration Theory for Visual-Inertial Navigation}, @@ -289,3 +298,17 @@ @article{Rehder2017Sensors number = 11, pages = {3257--3268} } +@techreport{Li2012TR, + title = {Consistency of EKF-Based Visual-Inertial Odometry}, + author = {Mingyang Li and Anastasios I. Mourikis}, + year = 2012, + note = {Available: \url{https://intra.ece.ucr.edu/~mourikis/tech_reports/VIO.pdf}}, + institution = {Dept. of Electrical Engineering, University of California, Riverside} +} +@techreport{Hesch2012TR, + title = {Observability-constrained Vision-aided Inertial Navigation}, + author = {Joel A. Hesch and Dimitrios G. Kottas and Sean L. Bowman and Stergios I. Roumeliotis}, + year = 2012, + note = {Available: \url{https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.721.6118&rep=rep1&type=pdf}}, + institution = {Dept. of Computer Science \& Engineering, University of Minnesota} +} diff --git a/docs/propagation-analytical.dox b/docs/propagation-analytical.dox index 6e54db1b7..f59a40487 100644 --- a/docs/propagation-analytical.dox +++ b/docs/propagation-analytical.dox @@ -4,17 +4,19 @@ @tableofcontents The state definition and general propagation equations are covered in the @ref propagation page. -Here we will cover the continuos-time integration of the state dynamics and inclusion of IMU intrinsic parameters. +Here we will cover the continuous-time integration of the state dynamics and inclusion of IMU intrinsic parameters. First we will cover how we can propagate our mean forward, afterwhich we cover how to derive the linearized error-state system propagation and approximations used. Key references for analytical inertial integration include: - Indirect Kalman Filter for 3D Attitude Estimation @cite Trawny2005TR -- Consistency Analysis and Improvement of Vision-aided Inertial Navigation @cite Hesch2013TRO +- Consistency Analysis and Improvement of Vision-aided Inertial Navigation @cite Hesch2013TRO @cite Hesch2012TR +- High-precision, consistent EKF-based visual-inertial odometry @cite Li2013IJRR @cite Li2012TR - Closed-form preintegration methods for graph-based visual-inertial navigation @cite Eckenhoff2019IJRR @cite Eckenhoff2018TR - Analytic Combined IMU Integration (ACI^2) for Visual-Inertial Navigation @cite Yang2020ICRA @cite Yang2019TR_ACI + @section imu_intrinsic_models IMU Intrinsic Models First we can re-write our IMU measurement models to find the the true (or corrected) @@ -32,16 +34,15 @@ This is how we will relate incoming measurements to IMU dynamics (see @ref imu_k \left( {}^a\mathbf{a}_{m}(t) - \mathbf{b}_a(t) - \mathbf{n}_a(t) \right) - - {}^I_G\mathbf{R}(t) {^G\mathbf{g}} \f} where \f$\mathbf{D}_w = \mathbf{T}^{-1}_w\f$ and \f$\mathbf{D}_a = \mathbf{T}^{-1}_a\f$. +Note that here we have factored out the \f$- {}^I_G\mathbf{R}(t) {^G\mathbf{g}}\f$ gravity term (see below state evolution equations). In practice, we calibrate \f$\mathbf{D}_a\f$, \f$\mathbf{D}_w\f$, \f${}^I_a\mathbf{R}\f$ (or \f${}^I_w\mathbf{R}\f$) and \f$\mathbf{T}_g\f$ -to prevent the need to have an unneeded matrix inversion / transposes in the measurement equation. - -We only calibrate either \f${}^I_w\mathbf{R}\f$ or \f${}^I_a\mathbf{R}\f$ since the base ``inertial'' -frame must coincide with one frame arbitrarily (would otherwise introduce too many degrees of freedom). +to prevent unneeded matrix inversions and transposes in the measurement equation. +We only calibrate either \f${}^I_w\mathbf{R}\f$ or \f${}^I_a\mathbf{R}\f$ since the base "inertial" +frame must coincide with one frame arbitrarily. If both \f${}^I_w\mathbf{R}\f$ and \f${}^I_a\mathbf{R}\f$ are calibrated, it will make the rotation between the IMU and camera unobservable due to over parameterization @cite Yang2020RSS. We define two different models of interested: @@ -53,7 +54,7 @@ We define two different models of interested: - *RPNG*: Contains \f$\mathbf{D}_{w6}\f$, \f$\mathbf{D}_{a6}\f$, the rotation \f${}^I_a\mathbf{R}\f$, and gravity sensitivity \f$\mathbf{T}_{g9}\f$. \f$\mathbf{D}_{a6}\f$ and \f$\mathbf{D}_{w6}\f$ are uptriangular matrices, and follows *imu2* analysed in @cite Yang2020RSS. -where we can define \f$\mathbf{D}_{*6} = {\mathbf{D}'_{*6}}^\top\f$ and the following matrices: +We can define \f$\mathbf{D}_{*6} = {\mathbf{D}'_{*6}}^\top\f$ and the following matrices: \f{align*}{ \mathbf{D}_{*6} &= \begin{bmatrix} @@ -78,8 +79,7 @@ in the IMU models, including scale/axis correction for gyroscope \f$\mathbf{D}_w scale/axis correction for accelerometer \f$\mathbf{D}_a\f$ (6 parameters), rotation from gyroscope to IMU frame \f${}^I_w\mathbf{R}\f$, rotation from accelerometer to IMU frame \f${}^I_a\mathbf{R}\f$ and gravity sensitivity \f$\mathbf{T}_g\f$ (9 parameters). -The state vector \f$\mathbf{x}\f$ contains the IMU state \f$\mathbf{x}_{I}\f$ and its intrinsics -\f$\mathbf{x}_{in}\f$: +The state vector contains the IMU state and its intrinsics: \f{align*}{ \mathbf{x}_I & = @@ -105,7 +105,7 @@ The state vector \f$\mathbf{x}\f$ contains the IMU state \f$\mathbf{x}_{I}\f$ an where \f${}^I_G\bar{q}\f$ denotes the rotation matrix \f${}^I_G\mathbf{R}\f$ from \f$\{G\}\f$ to \f$\{I\}\f$. \f${}^G\mathbf{p}_{I}\f$ and \f${}^G\mathbf{v}_I\f$ denote the IMU position and velocity in \f$\{G\}\f$. \f$\mathbf{x}_n\f$ denotes the IMU navigation states and \f$\mathbf{x}_b\f$ denotes the IMU biases. -IMU intrinsics, \f$\mathbf{x}_{in}\f$, contains the non-zero elements stored column-wise: +The IMU intrinsics, \f$\mathbf{x}_{in}\f$, contain the non-zero elements stored column-wise: \f{align*}{ \mathbf{x}_{D*} & = @@ -119,12 +119,814 @@ IMU intrinsics, \f$\mathbf{x}_{in}\f$, contains the non-zero elements stored col \f} -@section analytical_prop Analytical State Integration +@section analytical_prop Analytical State Mean Integration + +First we redefine the continuous-time integration equations mentioned in the @ref conti_prop section. +They are summarized as follows: + +\f{align*}{ +{}^{I_{k+1}}_{G}\mathbf{R} & + \triangleq\Delta \mathbf{R}_k ~{}^{I_k}_{G}\mathbf{R} \\ +{}^G\mathbf{p}_{I_{k+1}} & + \triangleq + {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{p}_{k} + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k \\ +{}^G\mathbf{v}_{I_{k+1}} & + \triangleq {}^G\mathbf{v}_{I_k} + + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{v}_{k} + - {}^G\mathbf{g}\Delta t_k \\ + \mathbf{b}_{g_{k+1}} & = \mathbf{b}_{g_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wg}(t_\tau) ~ d \tau \\ + \mathbf{b}_{a_{k+1}} & = \mathbf{b}_{a_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wa}(t_\tau) ~ d \tau +\f} + +where \f$\Delta t_k = t_{k+1} - t_{k}\f$, +\f${}^{I_k}_{I_\tau}\mathbf{R} = \exp(\int^{t_\tau}_{t_k} {}^{I}\boldsymbol{\omega}(t_u) ~ d u ) \f$, +\f$\exp(\cdot)\f$ is the \f$SO(3)\f$ matrix exponential @cite Chirikjian2011, and vectors are evaluated at +their subscript timesteps (e.g. \f${}^G\mathbf{v}_{I_k}={}^G\mathbf{v}_{I}(t_k)\f$). +We have the following integration components: + +\f{align*}{ + \Delta \mathbf{R}_k &= + {}^{I_{k+1}}_{I_k}\mathbf{R} + = \exp + \left(-\int^{t_{k+1}}_{t_{k}} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau\right) \\ +\Delta \mathbf{p}_{k} &= + \int^{t_{k+1}}_{t_{k}} \int^{s}_{t_{k}} {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau d s \triangleq \boldsymbol{\Xi}_2 \cdot {}^{I_k} \hat{\mathbf{a}} \\ +\Delta \mathbf{v}_{k} &= + \int^{t_{k+1}}_{t_{k}} + {}^{I_k}_{I_\tau}\mathbf{R} {}^{I} \mathbf{a}(t_\tau) ~ d \tau \triangleq \boldsymbol{\Xi}_1 \cdot {}^{I_k} \hat{\mathbf{a}} +\f} + +where we define the following integrations: + +\f{align*}{ +\boldsymbol{\Xi}_1 & \triangleq + \int^{t_{k+1}}_{t_k} + \exp \left( + {}^{I_\tau}\hat{\boldsymbol{\omega}}\delta\tau + \right) + d{\tau} \\ +\boldsymbol{\Xi}_2 &\triangleq + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp \left( + {}^{I_\tau}\hat{\boldsymbol{\omega}}\delta\tau + \right) + d\tau ds +\f} + +where \f$\delta\tau = t_{\tau} - t_{k}\f$ and the \f$\boldsymbol{\Xi}_1\f$ and \f$\boldsymbol{\Xi}_2\f$ integration components + can be evaluated either analytically (see @ref analytical_integration_components) or numerically using RK4. +\f${}^{I_k}\hat{\boldsymbol{\omega}}\f$ and \f${}^{I_k}\hat{\mathbf{a}}\f$ are defined as (dropping the timestamp): + +\f{align*}{ + {}^I\hat{\boldsymbol{\omega}} & = {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_{w} {}^w\hat{\boldsymbol{\omega}} \\ + {}^w\hat{\boldsymbol{\omega}} & = {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g + = + \begin{bmatrix} + {}^w\hat{w}_1 & {}^w\hat{w}_2 & {}^w\hat{w}_3 + \end{bmatrix}^{\top} \\ + {}^I\hat{\mathbf{a}} &= {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_{a} {}^a\hat{\mathbf{a}} \\ + {}^a\hat{\mathbf{a}} & = {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a + = + \begin{bmatrix} + {}^a\hat{a}_1 & {}^a\hat{a}_2 & {}^a\hat{a}_3 + \end{bmatrix}^{\top} +\f} + +Under the assumption of constant \f${}^{I_k}\hat{\boldsymbol{\omega}}\f$ and \f${}^{I_k}\hat{\mathbf{a}}\f$, +the above equations are also constant over the time interval. +The mean propagation for the new state at \f$t_{k+1}\f$ can be written after taking the expectation: + +\f{align*}{ + {}^{I_{k+1}}_G\hat{\mathbf{R}} & + \simeq + \Delta \mathbf{R}_k + {}^{I_k}_G\hat{\mathbf{R}} + \\ + {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq + {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\Delta t_k + + + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{p}}_k + - \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k + \\ + {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{v}}_k - {}^G\mathbf{g}\Delta t_k + \\ + \hat{\mathbf{b}}_{g_{k+1}} & = \hat{\mathbf{b}}_{g_k} + \\ + \hat{\mathbf{b}}_{a_{k+1}} & = \hat{\mathbf{b}}_{a_k} +\f} + + + + + +@section analytical_linearization Model Linearization Derivations + +@subsection analytical_linearization_imu IMU Reading Linearization +We first define the \f${}^{I_k}\tilde{\boldsymbol{\omega}}\f$ and \f${}^{I_k}\tilde{\mathbf{a}}\f$ error states. +For \f${}^I\hat{\mathbf{a}}\f$ and \f${}^I\tilde{\mathbf{a}}\f$, we have: + +\f{align*}{ + {}^I{\mathbf{a}} & = {}^I\hat{\mathbf{a}} + {}^I\tilde{\mathbf{a}} \\ + &\simeq + {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a + \left( + {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a + \right) + + + {}^I_a\hat{\mathbf{R}} \mathbf{H}_{Da} \tilde{\mathbf{x}}_{Da} \notag \\ + &+ + \lfloor {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a + \left( + {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a + \right) \rfloor + \delta \boldsymbol{\theta}_{Ia} + - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \tilde{\mathbf{b}}_a + - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a {\mathbf{n}}_a + \\ + {}^I\hat{\mathbf{a}} & = {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a + \left( + {}^a\mathbf{a}_m - \hat{\mathbf{b}}_a + \right) + \\ + {}^I\tilde{\mathbf{a}} & \simeq + {}^I_a\hat{\mathbf{R}} \mathbf{H}_{Da} \tilde{\mathbf{x}}_{Da} + + + \lfloor {}^I\hat{\mathbf{a}} \rfloor + \delta \boldsymbol{\theta}_{Ia} + - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \tilde{\mathbf{b}}_a + - {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a {\mathbf{n}}_a +\f} + +For \f${}^I\hat{\boldsymbol{\omega}}\f$ and \f${}^I\tilde{\boldsymbol{\omega}}\f$, we have: + +\f{align*}{ + {}^{I}\boldsymbol{\omega} & = {}^{I}\hat{\boldsymbol{\omega}} + + {}^{I}\tilde{\boldsymbol{\omega}} + \\ + & + \simeq + {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w + \left( + {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g + \right) + + \lfloor + {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w + \left( + {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g + \right) + \rfloor \delta \boldsymbol{\theta}_{Iw} + \\ + & + + {}^I_w\hat{\mathbf{R}} \mathbf{H}_{Dw} \tilde{\mathbf{x}}_{Dw} + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w + \left( + \hat{\mathbf{T}}_g {}^I\tilde{\mathbf{a}} + + \mathbf{H}_{Tg}\tilde{\mathbf{x}}_{Tg} + \right) + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w + \left( + \tilde{\mathbf{b}}_g + +\mathbf{n}_g + \right) + \notag + \\ + {}^{I}\hat{\boldsymbol{\omega}} & = + {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w + \left( + {}^w\boldsymbol{\omega}_m - \hat{\mathbf{T}}_g {}^I\hat{\mathbf{a}}-\hat{\mathbf{b}}_g + \right) + \\ + {}^{I}\tilde{\boldsymbol{\omega}} & \simeq + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \tilde{\mathbf{b}}_g + + {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \tilde{\mathbf{b}}_a + + {}^I_w\hat{\mathbf{R}}\mathbf{H}_{Dw}\tilde{\mathbf{x}}_{Dw} \notag \\ + & + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}}\mathbf{H}_{Da}\tilde{\mathbf{x}}_{Da} + + \lfloor {}^I\hat{\boldsymbol{\omega}} \rfloor \delta \boldsymbol{\theta}_{Iw} + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g + \lfloor {}^I\hat{\mathbf{a}} \rfloor \delta \boldsymbol{\theta}_{Ia} \notag \\ + & + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \mathbf{H}_{Tg} + \tilde{\mathbf{x}}_{Tg} - {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \mathbf{n}_g + + {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}}\hat{\mathbf{D}}_a \mathbf{n}_a +\f} + +where we need to define \f$\mathbf{H}_{Dw}\f$, \f$\mathbf{H}_{Da}\f$ and \f$\mathbf{H}_{Tg}\f$. +For the KALIBR model, we have: + +\f{align*}{ +\mathbf{H}_{Dw} & = + \begin{bmatrix} + {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_3 \mathbf{e}_3 + \end{bmatrix} \\ +\mathbf{H}_{Da} & = + \begin{bmatrix} + {}^a\hat{a}_1 \mathbf{I}_3 & + & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 + \end{bmatrix} \\ +\mathbf{H}_{Tg} & = + \begin{bmatrix} + {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 + \end{bmatrix} +\f} + +For the RPNG model, we have: + +\f{align*}{ +\mathbf{H}_{Dw} & = + \begin{bmatrix} + {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 + \end{bmatrix} \\ +\mathbf{H}_{Da} & = + \begin{bmatrix} + {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 + \end{bmatrix} \\ +\mathbf{H}_{Tg} & = + \begin{bmatrix} + {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 + \end{bmatrix} +\f} + +By summarizing the above equations, we have: + +\f{align*}{ + \begin{bmatrix} + {}^{I_k}\tilde{\boldsymbol{\omega}} \\ + {}^{I_k}\tilde{\mathbf{a}} + \end{bmatrix} + & = + \begin{bmatrix} + \mathbf{H}_b & \mathbf{H}_{in} + \end{bmatrix} + \begin{bmatrix} + \tilde{\mathbf{x}}_{b} \\ + \tilde{\mathbf{x}}_{in} + \end{bmatrix} + + + \mathbf{H}_n + \begin{bmatrix} + \mathbf{n}_{dg} \\ + \mathbf{n}_{da} + \end{bmatrix} +\f} + +where we have defined: + +\f{align*}{ +\mathbf{H}_b & = \mathbf{H}_n = + \begin{bmatrix} + -{}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w & + {}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w + \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_a \\ + \mathbf{0}_3 & -{}^I_a\hat{\mathbf{R}}\hat{\mathbf{D}}_a + \end{bmatrix} \\ +\mathbf{H}_{in} & = + \begin{bmatrix} + \mathbf{H}_w & \mathbf{H}_a & \mathbf{H}_{Iw} & \mathbf{H}_{Ia} & \mathbf{H}_{g} + \end{bmatrix} +\f} +\f{align*}{ + \mathbf{H}_w & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial \mathbf{x}_{Dw} } = + \begin{bmatrix} + {}^I_w\hat{\mathbf{R}} \mathbf{H}_{Dw} \\ + \mathbf{0}_3 + \end{bmatrix} + \\ + \mathbf{H}_a & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial \mathbf{x}_{Da} } = + \begin{bmatrix} + - {}^I_w\hat{\mathbf{R}}\hat{\mathbf{D}}_w \hat{\mathbf{T}}_g {}^I_a\hat{\mathbf{R}}\mathbf{H}_{Da} \\ + {}^I_a\hat{\mathbf{R}}\mathbf{H}_{Da} + \end{bmatrix} + \\ + \mathbf{H}_{Iw} & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial {}^I_w\delta \boldsymbol{\theta} } = + \begin{bmatrix} + \lfloor {}^{I}\hat{\boldsymbol{\omega}} \rfloor \\ + \mathbf{0}_3 + \end{bmatrix} + \\ + \mathbf{H}_{Ia} & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial {}^I_a\delta \boldsymbol{\theta} } = + \begin{bmatrix} + -{}^I_a\hat{\mathbf{R}} \hat{\mathbf{D}}_w \hat{\mathbf{T}}_g + \lfloor {}^{I}\hat{\mathbf{a}} \rfloor \\ + \lfloor {}^I\hat{\mathbf{a}} \rfloor + \end{bmatrix} + \\ + \mathbf{H}_{g} & = \frac{\partial [{}^{I_k}\tilde{\boldsymbol{\omega}} ~{}^{I_k}\tilde{\mathbf{a}}]}{\partial \mathbf{x}_{Tg} } = + \begin{bmatrix} + -{}^I_w\hat{\mathbf{R}} \hat{\mathbf{D}}_w \mathbf{H}_{Tg} \\ + \mathbf{0}_3 + \end{bmatrix} +\f} + + +@subsection analytical_linearization_state Inertial State Linearization +We then linearize \f$\Delta \mathbf{R}_k\f$, \f$\Delta\mathbf{p}_k\f$ and \f$\Delta \mathbf{v}_k\f$. +For the \f$\Delta \mathbf{R}_k\f$, we can get: + +\f{align*}{ + \Delta \mathbf{R}_k &= + \exp(-{}^{I_{k}}\boldsymbol{\omega}\Delta t_k) + \\ + &= \exp(-({}^{I_{k}}\hat{\boldsymbol{\omega}} + + {}^{I_{k}}\tilde{\boldsymbol{\omega}})\Delta t_k) + \\ + &\simeq + \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) + \cdot + \exp(-\mathbf{J}_r (-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) + {}^{I_{k}}\tilde{\boldsymbol{\omega}} + \Delta t_k) \\ + &\simeq + \underbrace{\exp(-\Delta \hat{\mathbf{R}}_k\mathbf{J}_r (-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) + {}^{I_{k}}\tilde{\boldsymbol{\omega}} + \Delta t_k)}_{\Delta \tilde{\mathbf{R}}_k} + \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) +\f} + +For the integration of \f$\Delta \mathbf{p}_k\f$, we get: + +\f{align*}{ + \Delta \mathbf{p}_{k} & = + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp({}^{I_{k}}\boldsymbol{\omega}\delta \tau) d \tau d s + \cdot + {}^{I_{k}} \mathbf{a} + \\ + & = + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp(({}^{I_{k}} \hat{\boldsymbol{\omega}} + + {}^{I_{k}}\tilde{\boldsymbol{\omega}})\delta \tau) d \tau d s + \cdot + ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) + \\ + & \simeq + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) \exp(\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau) d \tau d s + \cdot + ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) + \\ + & \simeq + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) (\mathbf{I} + \lfloor + {\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau} \rfloor) d \tau d s + \cdot + ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) + \\ + & \simeq + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau d s + \cdot + {}^{I_{k}} \hat{\mathbf{a}} + \notag + \\ + & + ~~~ - + \underbrace{ + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) + \lfloor {}^{I_{k}} \hat{\mathbf{a}} \rfloor + \mathbf{J}_r ({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) + d \tau d s}_{\boldsymbol{\Xi}_4} + \cdot + {}^{I_{k}}\tilde{\boldsymbol{\omega}} + \notag + \\ + & + ~~~ + + \underbrace{\int^{t_{k+1}}_{t_k} \int^{s}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) + d \tau d s }_{\boldsymbol{\Xi}_2} + \cdot + {}^{I_{k}}\tilde{\mathbf{a}} + \notag + \\& + = + \Delta \hat{\mathbf{p}}_{k} \underbrace{- + \boldsymbol{\Xi}_4 {}^{I_{k}}\tilde{\boldsymbol{\omega}} + +\boldsymbol{\Xi}_2 {}^{I_{k}}\tilde{\mathbf{a}}}_{\Delta \tilde{\mathbf{p}}_k} + \\ + & = \Delta \hat{\mathbf{p}}_{k} + \Delta \tilde{{\mathbf{p}}}_{k} +\f} + +For the integration of \f$\Delta \mathbf{v}_k\f$, we get: + +\f{align*}{ + \Delta \mathbf{v}_{k} + & = + \int^{t_{k+1}}_{t_k} + \exp({}^{I_{k}}\boldsymbol{\omega}\delta \tau) d \tau + \cdot + {}^{I_{k}} \mathbf{a} + \\ + & = + \int^{t_{k+1}}_{t_k} + \exp(({}^{I_{k}} \hat{\boldsymbol{\omega}} + {}^{I_{k}}\tilde{\boldsymbol{\omega}})\delta \tau) d \tau + \cdot + ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) + \\ + & \simeq + \int^{t_{k+1}}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) \exp(\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau) d \tau + \cdot + ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) + \\ + & \simeq + \int^{t_{k+1}}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) (\mathbf{I} + \lfloor + {\mathbf{J}_r({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) {}^{I_{k}}\tilde{\boldsymbol{\omega}}\delta \tau} \rfloor) d \tau + \cdot + ({}^{I_{k}}\hat{\mathbf{a}} + {}^{I_{k}}\tilde{\mathbf{a}}) + \\ + & \simeq + \int^{t_{k+1}}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) d \tau + \cdot + {}^{I_{k}} \hat{\mathbf{a}} + \notag + \\ + & + ~~~ - + \underbrace{ + \int^{t_{k+1}}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) + \lfloor {}^{I_{k}} \hat{\mathbf{a}} \rfloor + \mathbf{J}_r ({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) + d \tau }_{\boldsymbol{\Xi}_3} + \cdot + {}^{I_{k}}\tilde{\boldsymbol{\omega}} + \notag + \\ + & + ~~~ + + \underbrace{\int^{t_{k+1}}_{t_k} + \exp({}^{I_{k}}\hat{\boldsymbol{\omega}}\delta \tau) + d \tau }_{\boldsymbol{\Xi}_1} + \cdot + {}^{I_{k}}\tilde{\mathbf{a}} + \notag + \\& + = + \Delta \hat{\mathbf{v}}_{k} \underbrace{- + \boldsymbol{\Xi}_3 {}^{I_{k}}\tilde{\boldsymbol{\omega}} + +\boldsymbol{\Xi}_1 {}^{I_{k}}\tilde{\mathbf{a}}}_{\Delta \tilde{\mathbf{v}}_k} + \\ + & = \Delta \hat{\mathbf{v}}_{k} + \Delta \tilde{{\mathbf{v}}}_{k} +\f} + +where \f$\delta \tau = t_\tau - t_k\f$ and +\f$\boldsymbol{\Xi}_i, i=1\ldots4\f$ are shown in the @ref analytical_integration_components section. +\f$\mathbf {J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\f$ is the right Jacobian of \f${SO}(3)\f$ +[see @ref ov_core::Jr_so3() and @cite Barfoot2017]. +In summary, we have the following linearized integration components (see @ref analytical_integration_components): + +\f{align*}{ + \Delta \mathbf{R}_k & = + \Delta \tilde{\mathbf{R}}_k \Delta \hat{\mathbf{R}}_k + \triangleq + \exp \left( + -\Delta \hat{\mathbf{R}}_k + \mathbf{J}_r (\Delta \boldsymbol{\theta}_k) + {}^{I_k}\tilde{\boldsymbol{\omega}}\Delta t_k + \right) + \Delta \hat{\mathbf{R}}_k + \\ + \Delta \mathbf{p}_k & = + \Delta \hat{\mathbf{p}}_k + \Delta \tilde{\mathbf{p}}_k + \triangleq + \Delta \hat{\mathbf{p}}_k -\boldsymbol{\Xi}_4 {}^{I_k}\tilde{\boldsymbol{\omega}} + + \boldsymbol{\Xi}_2 {}^{I_k}\tilde{\mathbf{a}} + \\ + \Delta \mathbf{v}_k & = + \Delta \hat{\mathbf{v}}_k + \Delta \tilde{\mathbf{v}}_k + \triangleq + \Delta \hat{\mathbf{v}}_k -\boldsymbol{\Xi}_3 {}^{I_k}\tilde{\boldsymbol{\omega}} + + \boldsymbol{\Xi}_1 {}^{I_k}\tilde{\mathbf{a}} +\f} + +where \f$\mathbf{J}_r\left(\Delta \boldsymbol{\theta}_k\right) \triangleq \mathbf{J}_r \left(-{}^{I_k}\hat{\boldsymbol{\omega}}_k\Delta t_k \right)\f$ and \f$\boldsymbol{\Xi}_3\f$ and \f$\boldsymbol{\Xi}_4\f$ are defined as: + +\f{align*}{ + \boldsymbol{\Xi}_3 & = + \int^{t_{k+1}}_{t_k} {}^{I_k}_{I_{\tau}}\mathbf{R} + \lfloor {}^{I_{\tau}}\mathbf{a} \rfloor + \mathbf{J}_r \left( {}^{I_k}\hat{\boldsymbol{\omega}}_k \delta \tau \right) \delta \tau + ~d{\tau} + \\ + \boldsymbol{\Xi}_4 & = + \int^{t_{k+1}}_{t_k} \int^{s}_{t_k} {}^{I_k}_{I_{\tau}}\mathbf{R} + \lfloor {}^{I_{\tau}}\mathbf{a} \rfloor + \mathbf{J}_r \left( {}^{I_k}\hat{\boldsymbol{\omega}}_k \delta \tau \right) \delta \tau + ~d{\tau}ds +\f} + + + + + +@section analytical_linearization_sys Linearized Error-state Evolution + +Hence, the linearized IMU system is: + +\f{align*}{ + \delta \boldsymbol{\theta}_{k+1} & \simeq + \Delta \hat{\mathbf{R}}_k \delta \boldsymbol{\theta}_k + + \Delta \hat{\mathbf{R}}_k\mathbf{J}_r \left( \Delta \boldsymbol{\theta}_k \right) \Delta t_k {}^{I_k}\tilde{\boldsymbol{\omega}} + \\ + {}^G\tilde{\mathbf{p}}_{I_{k+1}} & \simeq + {}^G\tilde{\mathbf{p}}_{I_{k}}+{}^G\tilde{\mathbf{v}}_k\Delta t_k + - {}^{I_{k}}_G\hat{\mathbf{R}}^\top + \lfloor \Delta \hat{\mathbf{p}}_k \rfloor + \delta \boldsymbol{\theta}_k + + {}^{I_{k}}_G\hat{\mathbf{R}}^\top \left( -\boldsymbol{\Xi}_4 {}^{I_k}\tilde{\boldsymbol{\omega}} + \boldsymbol{\Xi}_2 {}^{I_k}\tilde{\mathbf{a}} \right) + \\ + {}^G\tilde{\mathbf{v}}_{I_{k+1}} & \simeq + {}^G\tilde{\mathbf{v}}_{I_{k}} + - {}^{I_{k}}_G\hat{\mathbf{R}}^\top + \lfloor \Delta \hat{\mathbf{v}}_k \rfloor + \delta \boldsymbol{\theta}_k + + {}^{I_{k}}_G\hat{\mathbf{R}}^\top \left( -\boldsymbol{\Xi}_3 {}^{I_k}\tilde{\boldsymbol{\omega}} + \boldsymbol{\Xi}_1 {}^{I_k}\tilde{\mathbf{a}} \right) +\f} + +The overall linearized error state system is: + +\f{align*}{ + \tilde{\mathbf{x}}_{I_{k+1}} & \simeq \boldsymbol{\Phi}_{I{(k+1,k)}}\tilde{\mathbf{x}}_{I_k} + \mathbf{G}_{Ik}\mathbf{n}_{dk} + \\ + \boldsymbol{\Phi}_{I(k+1,k)} & = + \begin{bmatrix} + \boldsymbol{\Phi}_{nn} & + \boldsymbol{\Phi}_{wa} \mathbf{H}_b & + \boldsymbol{\Phi}_{wa} \mathbf{H}_{in} \\ + \mathbf{0}_{6\times 9} & + \mathbf{I}_6 & + \mathbf{0}_{6\times 24} \\ + \mathbf{0}_{24\times 9} & \mathbf{0}_{24\times 6} & \mathbf{I}_{24} + \end{bmatrix} + \\ + \mathbf{G}_{Ik} & = + \begin{bmatrix} + \boldsymbol{\Phi}_{wa} \mathbf{H}_n & \mathbf{0}_{9\times 6} \\ + \mathbf{0}_{6} & \mathbf{I}_6 \Delta t_k \\ + \mathbf{0}_{24\times 6} & \mathbf{0}_{24\times 6} + \end{bmatrix} +\f} + +where \f$\boldsymbol{\Phi}_{I(k+1,k)}\f$ and \f$\mathbf{G}_{Ik}\f$ are the state transition matrix and noise Jacobians for IMU dynamic model; +\f$\mathbf{H}_{b}$, $\mathbf{H}_{in}$ and $\mathbf{H}_n\f$ are Jacobians related to bias, IMU intrinsics and noises, which can be found from previous derivations; +\f$\mathbf{n}_{dk} = [\mathbf{n}^{\top}_{dg} ~ \mathbf{n}^{\top}_{da} ~ \mathbf{n}^{\top}_{dwg} ~ \mathbf{n}^{\top}_{dwa}]^\top\f$ is the discrete-time IMU noises; +\f$\boldsymbol{\Phi}_{nn}\f$ and \f$\boldsymbol{\Phi}_{wa}\f$ can be computed as: + +\f{align*}{ + \boldsymbol{\Phi}_{nn} & = + \begin{bmatrix} + \Delta \hat{\mathbf{R}}_k & \mathbf{0}_3 & \mathbf{0}_3 \\ + -{}^{I_k}_G\hat{\mathbf{R}}^\top\lfloor \Delta \hat{\mathbf{p}}_k \rfloor & \mathbf{I}_3 & \mathbf{I}_3 \Delta t_k \\ + -{}^{I_k}_G\hat{\mathbf{R}}^\top\lfloor \Delta \hat{\mathbf{v}}_k \rfloor & \mathbf{0}_3 & \mathbf{I}_3 + \end{bmatrix} + \\ + \boldsymbol{\Phi}_{wa} & = + \begin{bmatrix} + \Delta \hat{\mathbf{R}}_k\mathbf{J}_r(\Delta \boldsymbol{\theta}_k) \Delta t_k & \mathbf{0}_3 \\ + -{}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_4 & {}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_2 \\ + -{}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_3 & {}^{I_k}_G\hat{\mathbf{R}}^\top\boldsymbol{\Xi}_1 + \end{bmatrix} +\f} + +When using FEJ (see @ref fej for an overview) for the state transition matrix, +we need to replace \f$\Delta \hat{\mathbf{R}}_k\f$, \f$\Delta \hat{\mathbf{p}}_k\f$ and \f$\Delta \hat{\mathbf{v}}_k\f$ as: + +\f{align*}{ + \Delta \hat{\mathbf{R}}_k & = + {}^{I_{k+1}}_{G}\hat{\mathbf{R}} + {}^{I_k}_G\hat{\mathbf{R}}^{\top} + \\ + \Delta \hat{\mathbf{p}}_k & = + {}^{I_k}_G\hat{\mathbf{R}} + ( + {}^G\hat{\mathbf{p}}_{I_{k+1}} - + {}^{G}\hat{\mathbf{p}}_{I_k} - {}^G\hat{\mathbf{v}}_{I_k}\Delta t_k + + \frac{1}{2}{}^G\mathbf{g}\Delta t^2_k + ) + \\ + \Delta \hat{\mathbf{v}}_k + & = + {}^{I_k}_G\hat{\mathbf{R}} + ( + {}^G\hat{\mathbf{v}}_{I_{k+1}} - + {}^{G}\hat{\mathbf{v}}_{I_k} + + {}^G\mathbf{g}\Delta t_k + ) +\f} + +Note that \f$\mathbf{n}_{d*}\sim \mathcal{N}(\mathbf{0}, \frac{\sigma^2_{*}\mathbf{I}_3}{\Delta t})\f$ and hence the covariance for \f$\mathbf{n}_{dI}\f$ can be written as: + +\f{align*}{ + \mathbf{Q}_{dI} & = + \begin{bmatrix} + \frac{\sigma^2_g}{\Delta t_k} \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ + \mathbf{0}_3 & \frac{\sigma^2_a}{\Delta t_k} \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ + \mathbf{0}_3 & \mathbf{0}_3 & \frac{\sigma^2_{wg}}{\Delta t_k} \mathbf{I}_3 & \mathbf{0}_3 \\ + \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \frac{\sigma^2_{wa}}{\Delta t_k} \mathbf{I}_3 + \end{bmatrix} +\f} + +Finally, the covariance propagation can be derived as: + +\f{align*}{ + \mathbf{P}_{k+1} & = + \boldsymbol{\Phi}_{I(k+1,k)} \mathbf{P}_k + \boldsymbol{\Phi}^{\top}_{I(k+1,k)} + + \mathbf{G}_{Ik} + \mathbf{Q}_{dI} + \mathbf{G}^{\top}_{Ik} +\f} -Consider the following +@section analytical_integration_components Integration Component Definitions + +When deriving the components, we drop the subscripts for simplicity. +As we define angular velocity \f$\hat{\omega} = ||{{}^I\hat{\boldsymbol{\omega}}}||\f$, +rotation axis \f$\mathbf{k} = {{{}^I\hat{\boldsymbol{\omega}}}}/{||{{}^I\hat{\boldsymbol{\omega}}}||}\f$, +and the small interval we are integrating over as \f$\delta t\f$. +The first integration we need is: + +\f{align*}{ + \boldsymbol{\Xi}_1 & = + \mathbf{I}_3 \delta t + + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} + \lfloor \hat{\mathbf{k}} \rfloor + + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 +\f} + +The second integration we need is: + +\f{align*}{ + \boldsymbol{\Xi}_2 & = + \frac{1}{2} \delta t^2 \mathbf{I}_3 + + \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor + + + \left( + \frac{1}{2} \delta t^2 - + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} + \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 +\f} + +The third integration can be written as: + +\f{align*}{ + \boldsymbol{\Xi}_3 + &= + \frac{1}{2}\delta t^2 + \lfloor \hat{\mathbf{a}} \rfloor + + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} + \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + \notag \\ + & + + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} + \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + \notag \\ + & + + \left( + \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} + \right) + \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor ^2 + \notag \\ + & + + + \left( + \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} + \right) + \lfloor \hat{\mathbf{k}} \rfloor ^2 \lfloor\hat{\mathbf{a}} \rfloor + \notag \\ + & + + + \left( + \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} + \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + \notag \\ + & + - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 +\f} + +The fourth integration we need is: + +\f{align*}{ + \boldsymbol{\Xi}_4 + & + = \frac{1}{6}\delta t^3 \lfloor\hat{\mathbf{a}} \rfloor + + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} + \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + \nonumber \\ & + + \left( + \frac{2(1- \cos(\hat{\omega} \delta t)) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t)}{\hat{\omega}^3} + \right) + \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + \nonumber \\ & + + \left( + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t}{\hat{\omega}^3} + + \frac{\delta t^3}{6} + \right) + \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + \nonumber \\ & + + + \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} + \lfloor \hat{\mathbf{k}} \rfloor^2\lfloor\hat{\mathbf{a}} \rfloor + \nonumber \\ & + + + \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + \nonumber \\ & + + + \frac{4 \cos(\hat{\omega} \delta t) - 4 + (\hat{\omega} \delta t)^2 + \hat{\omega} \delta t \sin(\hat{\omega} \delta t) } + {\hat{\omega}^3} + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 +\f} + +When \f$\hat{\omega}\f$ is too small, in order to avoid numerical instability, we can compute the above integration +identities as (see [L'Hôpital's rule](https://en.wikipedia.org/wiki/L%27H%C3%B4pital%27s_rule)): + +\f{align*}{ + \lim\limits_{\hat{\omega} \rightarrow 0} + \boldsymbol{\Xi}_1 + & = + \delta t \mathbf{I}_3 + + \delta t \sin (\hat{\omega} \delta t_i) + \lfloor \hat{\mathbf{k}} \rfloor + + + \delta t + \left( + 1 - \cos(\hat{\omega} \delta t) + \right) + \lfloor \hat{\mathbf{k}} \rfloor^2 + \\ + \lim\limits_{\hat{\omega} \rightarrow 0} + \boldsymbol{\Xi}_2 + & = + \frac{\delta t^2}{2} \mathbf{I}_3 + + \frac{\delta t^2}{2} \sin(\hat{\omega} \delta t) + \lfloor \hat{\mathbf{k}} \rfloor + + + \frac{\delta t^2}{2} + \left( + 1 - + \cos(\hat{\omega} \delta t) + \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 + \\ + & = \frac{\delta t}{2} \lim\limits_{\hat{\omega} \rightarrow 0} + \boldsymbol{\Xi}_1 + \\ + \lim\limits_{\hat{\omega} \rightarrow 0} + \boldsymbol{\Xi}_3 + & = + \frac{\delta t^2}{2} \lfloor \hat{\mathbf{a}} \rfloor + + + \frac{\delta t^2 \sin (\hat{\omega} \delta t)}{2} + \left( + - \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + + \lfloor \hat{\mathbf{k}} \rfloor \lfloor \hat{\mathbf{a}} \rfloor + + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 + \right) + \notag + \\ + & + \frac{\delta t^2}{2} (1 - \cos (\hat{\omega} \delta t)) + \left( + \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + + \lfloor \hat{\mathbf{k}} \rfloor^2 \lfloor \hat{\mathbf{a}} \rfloor + + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + \right) + \\ + \lim\limits_{\hat{\omega} \rightarrow 0} + \boldsymbol{\Xi}_4 + &= + \frac{\delta t^3}{6} \lfloor \hat{\mathbf{a}} \rfloor + + + \frac{\delta t^3 \sin (\hat{\omega} \delta t)}{6} + \left( + - \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + + \lfloor \hat{\mathbf{k}} \rfloor \lfloor \hat{\mathbf{a}} \rfloor + + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 + \right) + \notag + \\ + & + \frac{\delta t^3}{6} (1 - \cos (\hat{\omega} \delta t)) + \left( + \lfloor \hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + + \lfloor \hat{\mathbf{k}} \rfloor^2 \lfloor \hat{\mathbf{a}} \rfloor + + \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + \right) + \\ + & = \frac{\delta t}{3} \lim\limits_{\hat{\omega} \rightarrow 0} + \boldsymbol{\Xi}_3 +\f} + diff --git a/docs/propagation.dox b/docs/propagation.dox index 07594bfcb..e83b8914b 100644 --- a/docs/propagation.dox +++ b/docs/propagation.dox @@ -80,7 +80,7 @@ while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quatern \cos(\frac{1}{2}\tilde{\theta}) \end{bmatrix} \simeq \begin{bmatrix} - \frac{1}{2}\tilde{\boldsymbol{\theta}}\\ 1 + \frac{1}{2}\delta\tilde{\boldsymbol{\theta}}\\ 1 \end{bmatrix} \f} @@ -92,7 +92,7 @@ The total IMU error state thus is defined as the following 15x1 (not 16x1) vecto \f{align*}{ \tilde{\mathbf{x}}_I(t) = \begin{bmatrix} -^I_G\tilde{\boldsymbol{\theta}}(t) \\ +^I_G\delta\tilde{\boldsymbol{\theta}}(t) \\ ^G\tilde{\mathbf{p}}_I(t) \\ ^G\tilde{\mathbf{v}}_I(t)\\ \tilde{\mathbf{b}}_{{g}}(t) \\ @@ -140,7 +140,7 @@ we can define the solutions to the above IMU kinematics differential equation. \f{align*}{ {}^{I_{k+1}}_{G}\mathbf{R} & = \exp \left(-\int^{t_{k+1}}_{t_k} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau \right) {}^{I_k}_{G}\mathbf{R} \\ - &\triangleq \Delta \mathbf{R}_k {}^{I_k}_{G}\mathbf{R} \\ + &\triangleq \Delta \mathbf{R}_k ~{}^{I_k}_{G}\mathbf{R} \\ {}^G\mathbf{p}_{I_{k+1}} & = {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + {}^{I_k}_{G}\mathbf{R}^\top @@ -158,8 +158,8 @@ we can define the solutions to the above IMU kinematics differential equation. &\triangleq {}^G\mathbf{v}_{I_k} + {}^{I_k}_{G}\mathbf{R}^\top \Delta \mathbf{v}_{k} - {}^G\mathbf{g}\Delta t_k \\ - \mathbf{b}_{g_{k+1}} & = \mathbf{b}_{g_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wg}(t) ~ d \tau \\ - \mathbf{b}_{a_{k+1}} & = \mathbf{b}_{a_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wa}(t) ~ d \tau + \mathbf{b}_{g_{k+1}} & = \mathbf{b}_{g_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wg}(t_\tau) ~ d \tau \\ + \mathbf{b}_{a_{k+1}} & = \mathbf{b}_{a_{k}} + \int^{t_{k+1}}_{t_k} \mathbf{n}_{wa}(t_\tau) ~ d \tau \f} where \f$\Delta t_k = t_{k+1} - t_{k}\f$, diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 665724195..4a4e6ccd9 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -19,7 +19,7 @@ - + @@ -81,7 +81,6 @@ - diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 7e02df759..836f7fda6 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -220,7 +220,8 @@ void Propagator::fast_state_propagate(std::shared_ptr state, double times state_plus.block(7, 0, 3, 1) = state->_imu->vel(); if (!prop_data.empty()) { Eigen::Vector3d last_a = state->_calib_imu_ACCtoIMU->Rot() * Da * (prop_data.at(prop_data.size() - 1).am - state->_imu->bias_a()); - Eigen::Vector3d last_w = state->_calib_imu_GYROtoIMU->Rot() * Dw * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - Tg * last_a); + Eigen::Vector3d last_w = + state->_calib_imu_GYROtoIMU->Rot() * Dw * (prop_data.at(prop_data.size() - 1).wm - state->_imu->bias_g() - Tg * last_a); state_plus.block(10, 0, 3, 1) = last_w; } @@ -447,22 +448,14 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double dt, const E Eigen::Matrix3d sK2 = sK * sK; Eigen::Matrix3d sA = ov_core::skew_x(a_hat); - // Based on the delta theta, let's decide which integration will be used - bool small_w = (w_norm < 1.0 / 180 * M_PI / 2); - bool small_th = (d_th < 1.0 / 180 * M_PI / 2); - // Integration components will be used later - Eigen::Matrix3d R_k1tok, Xi_1, Xi_2, Jr_k1tok, Xi_3, Xi_4; - - // Now the R and its jacobian J can be computed - R_k1tok = I_3x3 + sin_dth * sK + (1.0 - cos_dth) * sK2; - if (!small_th) { - Jr_k1tok = I_3x3 - (1.0 - cos_dth) / d_th * sK + (d_th - sin_dth) / d_th * sK2; - } else { - Jr_k1tok = I_3x3 - sin_dth * sK + (1.0 - cos_dth) * sK2; - } + Eigen::Matrix3d R_ktok1, Xi_1, Xi_2, Jr_ktok1, Xi_3, Xi_4; + R_ktok1 = ov_core::exp_so3(-w_hat * dt); + Jr_ktok1 = ov_core::Jr_so3(-w_hat * dt); // Now begin the integration of each component + // Based on the delta theta, let's decide which integration will be used + bool small_w = (w_norm < 1.0 / 180 * M_PI / 2); if (!small_w) { // first order rotation integration with constant omega @@ -501,10 +494,10 @@ void Propagator::compute_Xi_sum(std::shared_ptr state, double dt, const E // Store the integrated parameters Xi_sum.setZero(); - Xi_sum.block(0, 0, 3, 3) = R_k1tok; + Xi_sum.block(0, 0, 3, 3) = R_ktok1; Xi_sum.block(0, 3, 3, 3) = Xi_1; Xi_sum.block(0, 6, 3, 3) = Xi_2; - Xi_sum.block(0, 9, 3, 3) = Jr_k1tok; + Xi_sum.block(0, 9, 3, 3) = Jr_ktok1; Xi_sum.block(0, 12, 3, 3) = Xi_3; Xi_sum.block(0, 15, 3, 3) = Xi_4; } @@ -515,13 +508,12 @@ void Propagator::predict_mean_analytic(std::shared_ptr state, double dt, // Pre-compute things Eigen::Matrix3d R_Gtok = state->_imu->Rot(); - Eigen::Matrix3d R_k1_to_k = Xi_sum.block(0, 0, 3, 3); - Eigen::Vector4d q_k_to_k1 = ov_core::rot_2_quat(R_k1_to_k.transpose()); + Eigen::Vector4d q_ktok1 = ov_core::rot_2_quat(Xi_sum.block(0, 0, 3, 3)); Eigen::Matrix3d Xi_1 = Xi_sum.block(0, 3, 3, 3); Eigen::Matrix3d Xi_2 = Xi_sum.block(0, 6, 3, 3); // Use our integrated Xi's to move the state forward - new_q = ov_core::quat_multiply(q_k_to_k1, state->_imu->quat()); + new_q = ov_core::quat_multiply(q_ktok1, state->_imu->quat()); new_v = state->_imu->vel() + R_Gtok.transpose() * Xi_1 * a_hat - _gravity * dt; new_p = state->_imu->pos() + state->_imu->vel() * dt + R_Gtok.transpose() * Xi_2 * a_hat - 0.5 * _gravity * dt * dt; } @@ -591,7 +583,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d Eigen::Matrix3d Xi_1 = Xi_sum.block(0, 3, 3, 3); Eigen::Matrix3d Xi_2 = Xi_sum.block(0, 6, 3, 3); - Eigen::Matrix3d Jr = Xi_sum.block(0, 9, 3, 3); + Eigen::Matrix3d Jr_ktok1 = Xi_sum.block(0, 9, 3, 3); Eigen::Matrix3d Xi_3 = Xi_sum.block(0, 12, 3, 3); Eigen::Matrix3d Xi_4 = Xi_sum.block(0, 15, 3, 3); @@ -608,13 +600,13 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d F.block(v_id, v_id, 3, 3).setIdentity(); // for bg - F.block(th_id, bg_id, 3, 3) = -Jr * dt * R_wtoI * Dw; + F.block(th_id, bg_id, 3, 3) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw; F.block(p_id, bg_id, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI * Dw; F.block(v_id, bg_id, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI * Dw; F.block(bg_id, bg_id, 3, 3).setIdentity(); // for ba - F.block(th_id, ba_id, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + F.block(th_id, ba_id, 3, 3) = dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * R_atoI * Da; F.block(p_id, ba_id, 3, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; F.block(v_id, ba_id, 3, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; F.block(ba_id, ba_id, 3, 3).setIdentity(); @@ -622,7 +614,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // begin to add the state transition matrix for the omega intrinsics Dw part if (Dw_id != -1) { Eigen::MatrixXd H_Dw = compute_H_Dw(state, w_uncorrected); - F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * H_Dw; + F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = dR_ktok1 * Jr_ktok1 * dt * R_wtoI * H_Dw; F.block(p_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_4 * R_wtoI * H_Dw; F.block(v_id, Dw_id, 3, state->_calib_imu_dw->size()) = -R_k.transpose() * Xi_3 * R_wtoI * H_Dw; F.block(Dw_id, Dw_id, state->_calib_imu_dw->size(), state->_calib_imu_dw->size()).setIdentity(); @@ -631,7 +623,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // begin to add the state transition matrix for the acc intrinsics Da part if (Da_id != -1) { Eigen::MatrixXd H_Da = compute_H_Da(state, a_uncorrected); - F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Dw * Tg * R_atoI * H_Da; + F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * R_atoI * H_Da; F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * H_Da; F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * H_Da; F.block(Da_id, Da_id, state->_calib_imu_da->size(), state->_calib_imu_da->size()).setIdentity(); @@ -640,7 +632,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // add the state transition matrix of the Tg part if (Tg_id != -1) { Eigen::MatrixXd H_Tg = compute_H_Tg(state, a_k); - F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * H_Tg; + F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * H_Tg; F.block(p_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_4 * R_wtoI * Dw * H_Tg; F.block(v_id, Tg_id, 3, state->_calib_imu_tg->size()) = R_k.transpose() * Xi_3 * R_wtoI * Dw * H_Tg; F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); @@ -648,7 +640,7 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // begin to add the state transition matrix for the R_ACCtoIMU part if (th_atoI_id != -1) { - F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block(th_id, th_atoI_id, 3, 3) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); F.block(p_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * ov_core::skew_x(a_k); F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); @@ -656,17 +648,17 @@ void Propagator::compute_F_and_G_analytic(std::shared_ptr state, double d // begin to add the state transition matrix for the R_GYROtoIMU part if (th_wtoI_id != -1) { - F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); + F.block(th_id, th_wtoI_id, 3, 3) = dR_ktok1 * Jr_ktok1 * dt * ov_core::skew_x(w_k); F.block(p_id, th_wtoI_id, 3, 3) = -R_k.transpose() * Xi_4 * ov_core::skew_x(w_k); F.block(v_id, th_wtoI_id, 3, 3) = -R_k.transpose() * Xi_3 * ov_core::skew_x(w_k); F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); } // construct the G part - G.block(th_id, 0, 3, 3) = -Jr * dt * R_wtoI * Dw; + G.block(th_id, 0, 3, 3) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw; G.block(p_id, 0, 3, 3) = R_k.transpose() * Xi_4 * R_wtoI * Dw; G.block(v_id, 0, 3, 3) = R_k.transpose() * Xi_3 * R_wtoI * Dw; - G.block(th_id, 3, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block(th_id, 3, 3, 3) = dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * R_atoI * Da; G.block(p_id, 3, 3, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_wtoI * Dw * Tg) * R_atoI * Da; G.block(v_id, 3, 3, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_wtoI * Dw * Tg) * R_atoI * Da; G.block(bg_id, 6, 3, 3) = dt * Eigen::Matrix3d::Identity(); @@ -736,13 +728,13 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d Eigen::Matrix3d R_wtoI = state->_calib_imu_GYROtoIMU->Rot(); Eigen::Vector3d a_k = R_atoI * Da * a_uncorrected; Eigen::Vector3d w_k = R_wtoI * Dw * w_uncorrected; // contains gravity correction already - Eigen::Matrix3d Jr = Jr_so3(w_k * dt); + Eigen::Matrix3d Jr_ktok1 = Jr_so3(log_so3(dR_ktok1)); // for theta F.block(th_id, th_id, 3, 3) = dR_ktok1; - // F.block(th_id, bg_id, 3, 3) = -Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; - F.block(th_id, bg_id, 3, 3) = -Jr * dt * R_wtoI * Dw; - F.block(th_id, ba_id, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + // F.block(th_id, bg_id, 3, 3) = -dR_ktok1 * Jr_so3(w_hat * dt) * dt * R_wtoI_fej * Dw_fej; + F.block(th_id, bg_id, 3, 3) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw; + F.block(th_id, ba_id, 3, 3) = dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * R_atoI * Da; // for position F.block(p_id, th_id, 3, 3) = -skew_x(new_p - p_k - v_k * dt + 0.5 * _gravity * dt * dt) * R_k.transpose(); @@ -764,14 +756,14 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the omega intrinsics Dw part if (Dw_id != -1) { Eigen::MatrixXd H_Dw = compute_H_Dw(state, w_uncorrected); - F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = Jr * dt * R_wtoI * H_Dw; + F.block(th_id, Dw_id, 3, state->_calib_imu_dw->size()) = dR_ktok1 * Jr_ktok1 * dt * R_wtoI * H_Dw; F.block(Dw_id, Dw_id, state->_calib_imu_dw->size(), state->_calib_imu_dw->size()).setIdentity(); } // begin to add the state transition matrix for the acc intrinsics Da part if (Da_id != -1) { Eigen::MatrixXd H_Da = compute_H_Da(state, a_uncorrected); - F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -Jr * dt * R_wtoI * Tg * R_atoI * H_Da; + F.block(th_id, Da_id, 3, state->_calib_imu_da->size()) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Tg * R_atoI * H_Da; F.block(p_id, Da_id, 3, state->_calib_imu_da->size()) = 0.5 * R_k.transpose() * dt * dt * R_atoI * H_Da; F.block(v_id, Da_id, 3, state->_calib_imu_da->size()) = R_k.transpose() * dt * R_atoI * H_Da; F.block(Da_id, Da_id, state->_calib_imu_da->size(), state->_calib_imu_da->size()).setIdentity(); @@ -780,13 +772,13 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the gravity sensitivity Tg part if (Tg_id != -1) { Eigen::MatrixXd H_Tg = compute_H_Tg(state, a_k); - F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -Jr * dt * R_wtoI * Dw * H_Tg; + F.block(th_id, Tg_id, 3, state->_calib_imu_tg->size()) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * H_Tg; F.block(Tg_id, Tg_id, state->_calib_imu_tg->size(), state->_calib_imu_tg->size()).setIdentity(); } // begin to add the state transition matrix for the R_ACCtoIMU part if (th_atoI_id != -1) { - F.block(th_id, th_atoI_id, 3, 3) = -Jr * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); + F.block(th_id, th_atoI_id, 3, 3) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * ov_core::skew_x(a_k); F.block(p_id, th_atoI_id, 3, 3) = 0.5 * R_k.transpose() * dt * dt * ov_core::skew_x(a_k); F.block(v_id, th_atoI_id, 3, 3) = R_k.transpose() * dt * ov_core::skew_x(a_k); F.block(th_atoI_id, th_atoI_id, 3, 3).setIdentity(); @@ -794,13 +786,13 @@ void Propagator::compute_F_and_G_discrete(std::shared_ptr state, double d // begin to add the state transition matrix for the R_GYROtoIMU part if (th_wtoI_id != -1) { - F.block(th_id, th_wtoI_id, 3, 3) = Jr * dt * ov_core::skew_x(w_k); + F.block(th_id, th_wtoI_id, 3, 3) = dR_ktok1 * Jr_ktok1 * dt * ov_core::skew_x(w_k); F.block(th_wtoI_id, th_wtoI_id, 3, 3).setIdentity(); } // Noise jacobian - G.block(th_id, 0, 3, 3) = -Jr * dt * R_wtoI * Dw; - G.block(th_id, 3, 3, 3) = Jr * dt * R_wtoI * Dw * Tg * R_atoI * Da; + G.block(th_id, 0, 3, 3) = -dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw; + G.block(th_id, 3, 3, 3) = dR_ktok1 * Jr_ktok1 * dt * R_wtoI * Dw * Tg * R_atoI * Da; G.block(v_id, 3, 3, 3) = -R_k.transpose() * dt * R_atoI * Da; G.block(p_id, 3, 3, 3) = -0.5 * R_k.transpose() * dt * dt * R_atoI * Da; G.block(bg_id, 6, 3, 3) = dt * Eigen::Matrix3d::Identity(); diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index f804af2a7..7583e826c 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -164,18 +164,13 @@ class Propagator { /** * @brief compute the Jacobians for Dw * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * For kalibr model + * See @ref analytical_linearization_imu for details. * \f{align*}{ - * \mathbf{H}_{Dw} & = + * \mathbf{H}_{Dw,kalibr} & = * \begin{bmatrix} - * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_2\mathbf{e}_3 - * \end{bmatrix} - * \f} - * - * For rpng model: - * \f{align*}{ - * \mathbf{H}_{Dw} & = + * {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_3 \mathbf{e}_3 + * \end{bmatrix} \\ + * \mathbf{H}_{Dw,rpng} & = * \begin{bmatrix} * {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 * \end{bmatrix} @@ -189,18 +184,13 @@ class Propagator { /** * @brief compute the Jacobians for Da * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * For kalibr model + * See @ref analytical_linearization_imu for details. * \f{align*}{ - * \mathbf{H}_{Da} & = + * \mathbf{H}_{Da,kalibr} & = * \begin{bmatrix} * {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 - * \end{bmatrix} - * \f} - * - * For rpng: - * \f{align*}{ - * \mathbf{H}_{Da} & = + * \end{bmatrix} \\ + * \mathbf{H}_{Da,rpng} & = * \begin{bmatrix} * {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 * \end{bmatrix} @@ -214,7 +204,7 @@ class Propagator { /** * @brief compute the Jacobians for Tg * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) + * See @ref analytical_linearization_imu for details. * \f{align*}{ * \mathbf{H}_{Tg} & = * \begin{bmatrix} @@ -239,8 +229,8 @@ class Propagator { * This function can be replaced with analytical/numerical integration or when using a different state representation. * This contains our state transition matrix along with how our noise evolves in time. * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref error_prop page for details on how discrete model was derived. - * See the @ref imu_intrinsic page for details on how analytic model was derived. + * See the @ref propagation_discrete page for details on how discrete model was derived. + * See the @ref propagation_analytical page for details on how analytic model was derived. * * @param state Pointer to state * @param data_minus imu readings at beginning of interval @@ -254,7 +244,7 @@ class Propagator { /** * @brief Discrete imu mean propagation. * - * See @ref propagation for details on these equations. + * See @ref disc_prop for details on these equations. * \f{align*}{ * \text{}^{I_{k+1}}_{G}\hat{\bar{q}} * &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) @@ -284,7 +274,7 @@ class Propagator { * See this wikipedia page on [Runge-Kutta Methods](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods). * We are doing a RK4 method, [this wolfram page](http://mathworld.wolfram.com/Runge-KuttaMethod.html) has the forth order equation * defined below. We define function \f$ f(t,y) \f$ where y is a function of time t, see @ref imu_kinematic for the definition of the - * continous-time functions. + * continuous-time functions. * * \f{align*}{ * {k_1} &= f({t_0}, y_0) \Delta t \\ @@ -311,17 +301,16 @@ class Propagator { /** * @brief Analytically compute the integration components based on ACI^2 * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf), [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) + * See the @ref analytical_prop page and @ref analytical_integration_components for details. + * For computing Xi_1, Xi_2, Xi_3 and Xi_4 we have: * - * For computing Xi_1, Xi_2, Xi_3 and Xi_4, you can refer to @ref imu_intrinsics or the following equations: * \f{align*}{ * \boldsymbol{\Xi}_1 & = \mathbf{I}_3 \delta t + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} \lfloor \hat{\mathbf{k}} \rfloor - * + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 + * + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 \\ * \boldsymbol{\Xi}_2 & = \frac{1}{2} \delta t^2 \mathbf{I}_3 + * \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor * + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 - * \boldsymbol{\Xi}_3 &= - * \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor + * \\ \boldsymbol{\Xi}_3 &= \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor * + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} \lfloor\hat{\mathbf{a}} \rfloor \lfloor * \hat{\mathbf{k}} \rfloor * + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} @@ -336,7 +325,8 @@ class Propagator { * \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} * \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor * - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) - * }{\hat{\omega}^2} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 \boldsymbol{\Xi}_4 & = \frac{1}{6}\delta + * }{\hat{\omega}^2} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 \\ + * \boldsymbol{\Xi}_4 & = \frac{1}{6}\delta * t^3 \lfloor\hat{\mathbf{a}} \rfloor * + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} * \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor @@ -365,7 +355,7 @@ class Propagator { * @param dt Time we should integrate over * @param w_hat Angular velocity with bias removed * @param a_hat Linear acceleration with bias removed - * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 in order + * @param Xi_sum All the needed integration components, including R_k, Xi_1, Xi_2, Jr, Xi_3, Xi_4 in order */ void compute_Xi_sum(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Matrix &Xi_sum); @@ -373,10 +363,10 @@ class Propagator { /** * @brief Analytically predict IMU mean based on ACI^2 * - * See the paper link: [RSS2020](https://yangyulin.net/papers/2020_rss.pdf), [ICRA2020](https://yangyulin.net/papers/2020_icra_aci.pdf) + * See the @ref analytical_prop page for details. * * \f{align*}{ - * {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}^{\top}_k {}^{I_k}_G\hat{\mathbf{R}} \\ + * {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}_k {}^{I_k}_G\hat{\mathbf{R}} \\ * {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\delta t_k + * {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{p}}_k - \frac{1}{2}{}^G\mathbf{g}\delta t^2_k \\ * {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{v}}_k - @@ -390,7 +380,7 @@ class Propagator { * @param new_q The resulting new orientation after integration * @param new_v The resulting new velocity after integration * @param new_p The resulting new position after integration - * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + * @param Xi_sum All the needed integration components, including R_k, Xi_1, Xi_2, Jr, Xi_3, Xi_4 */ void predict_mean_analytic(std::shared_ptr state, double dt, const Eigen::Vector3d &w_hat, const Eigen::Vector3d &a_hat, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p, Eigen::Matrix &Xi_sum); @@ -398,13 +388,10 @@ class Propagator { /** * @brief Analytically compute state transition matrix F and noise Jacobian G based on ACI^2 * - * See the paper link: [RSS20](https://yangyulin.net/papers/2020_rss.pdf) - * See the paper link: [ICRA20](https://yangyulin.net/papers/2020_icra_aci.pdf) - * - * This function is for analytical integration or when using a different state representation. + * This function is for analytical integration of the linearized error-state. * This contains our state transition matrix and noise Jacobians. * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref imu_intrinsics page for details on how this was derived. + * See the @ref analytical_linearization page for details on how this was derived. * * @param state Pointer to state * @param dt Time we should integrate over @@ -414,7 +401,7 @@ class Propagator { * @param new_q The resulting new orientation after integration * @param new_v The resulting new velocity after integration * @param new_p The resulting new position after integration - * @param Xi_sum All the needed integration components, including R, Xi_1, Xi_2, Jr, Xi_3, Xi_4 + * @param Xi_sum All the needed integration components, including R_k, Xi_1, Xi_2, Jr, Xi_3, Xi_4 * @param F State transition matrix * @param G Noise Jacobian */ @@ -429,7 +416,7 @@ class Propagator { * This function is for analytical integration or when using a different state representation. * This contains our state transition matrix and noise Jacobians. * If you have other state variables besides the IMU that evolve you would add them here. - * See the @ref imu_intrinsics page for details on how this was derived. + * See the @ref error_prop page for details on how this was derived. * * @param state Pointer to state * @param dt Time we should integrate over @@ -446,7 +433,6 @@ class Propagator { const Eigen::Vector3d &w_uncorrected, const Eigen::Vector3d &a_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v, const Eigen::Vector3d &new_p, Eigen::MatrixXd &F, Eigen::MatrixXd &G); - /// Container for the noise values NoiseManager _noises; From 068490cd1664a52fef5ed193ba25b3407e43ae2d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 9 May 2023 16:13:44 -0400 Subject: [PATCH 39/60] update configs to 2.7 format with intrinsics --- config/rpng_plane/estimator_config.yaml | 2 ++ config/rpng_plane/kalibr_imu_chain.yaml | 22 +++++++++++++++++++++- config/rpng_sim/kalibr_imu_chain.yaml | 2 +- config/rs_d455/estimator_config.yaml | 2 ++ config/rs_d455/kalibr_imu_chain.yaml | 22 +++++++++++++++++++++- config/rs_t265/estimator_config.yaml | 2 ++ config/rs_t265/kalibr_imu_chain.yaml | 22 +++++++++++++++++++++- config/uzhfpv_indoor/kalibr_imu_chain.yaml | 8 ++++---- 8 files changed, 74 insertions(+), 8 deletions(-) diff --git a/config/rpng_plane/estimator_config.yaml b/config/rpng_plane/estimator_config.yaml index cb4c23484..b1ee58b27 100644 --- a/config/rpng_plane/estimator_config.yaml +++ b/config/rpng_plane/estimator_config.yaml @@ -12,6 +12,8 @@ max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular ( calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 25 # number of features in our state vector diff --git a/config/rpng_plane/kalibr_imu_chain.yaml b/config/rpng_plane/kalibr_imu_chain.yaml index a1d0a3c12..38dab7bd4 100644 --- a/config/rpng_plane/kalibr_imu_chain.yaml +++ b/config/rpng_plane/kalibr_imu_chain.yaml @@ -10,7 +10,27 @@ imu0: accelerometer_random_walk: 0.00041327852 gyroscope_noise_density: 0.00020544166 gyroscope_random_walk: 1.110622e-05 - model: calibrated rostopic: /d455/imu time_offset: 0.0 update_rate: 400 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 61f3f5fb1..8a4d0bbe8 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -13,7 +13,7 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "rpng" # calibrated (same as kalibr), kalibr, rpng + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rs_d455/estimator_config.yaml b/config/rs_d455/estimator_config.yaml index 053c8a0cf..64d705ffd 100644 --- a/config/rs_d455/estimator_config.yaml +++ b/config/rs_d455/estimator_config.yaml @@ -12,6 +12,8 @@ max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular ( calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 50 # number of features in our state vector diff --git a/config/rs_d455/kalibr_imu_chain.yaml b/config/rs_d455/kalibr_imu_chain.yaml index a8ea0cb15..cdcf59533 100644 --- a/config/rs_d455/kalibr_imu_chain.yaml +++ b/config/rs_d455/kalibr_imu_chain.yaml @@ -19,7 +19,27 @@ imu0: accelerometer_random_walk: 0.00041327852 gyroscope_noise_density: 0.00020544166 gyroscope_random_walk: 0.00001110622 - model: calibrated rostopic: /d455/imu time_offset: 0.0 update_rate: 400 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/config/rs_t265/estimator_config.yaml b/config/rs_t265/estimator_config.yaml index 827e0c998..6cfad3214 100644 --- a/config/rs_t265/estimator_config.yaml +++ b/config/rs_t265/estimator_config.yaml @@ -12,6 +12,8 @@ max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular ( calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 50 # number of features in our state vector diff --git a/config/rs_t265/kalibr_imu_chain.yaml b/config/rs_t265/kalibr_imu_chain.yaml index f553b6349..876b54833 100644 --- a/config/rs_t265/kalibr_imu_chain.yaml +++ b/config/rs_t265/kalibr_imu_chain.yaml @@ -19,9 +19,29 @@ imu0: accelerometer_random_walk: 0.0006696534319519138 gyroscope_noise_density: 0.00054166825 gyroscope_random_walk: 1.1687657234840692e-05 - model: calibrated rostopic: /t265/imu time_offset: 0.0 update_rate: 200 + model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/config/uzhfpv_indoor/kalibr_imu_chain.yaml index 5338cb5c9..d02d12239 100644 --- a/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -10,10 +10,10 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) -# accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) -# accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) -# gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) -# gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 From eb463b29aecd7626cbba5a13f4557c04d9a27432 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 9 May 2023 16:16:55 -0400 Subject: [PATCH 40/60] fix #333 documention typos --- docs/eval-error.dox | 6 +++--- ov_eval/src/format_converter.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/eval-error.dox b/docs/eval-error.dox index 129b95ce7..ae5dc8c7b 100644 --- a/docs/eval-error.dox +++ b/docs/eval-error.dox @@ -38,8 +38,8 @@ By default the EuRoC groundtruth has the timestamp in nanoseconds and the quater A user can either process all CSV files in a given folder, or just a specific one. @code{.shell-session} -rosrun ov_eval format_convert folder/path/ -rosrun ov_eval format_convert file.csv +rosrun ov_eval format_converter folder/path/ +rosrun ov_eval format_converter file.csv @endcode In addition we have a specific folder structure that is assumed. @@ -98,7 +98,7 @@ It will use the filename as the name in the legend, so you can change that to ch @code{.shell-session} rosrun ov_eval plot_trajectories ... -rosrun ov_eval plot_trajectories posyaw 1565371553_estimate.txt truths/V1_01_easy.txt +rosrun ov_eval plot_trajectories posyaw truths/V1_01_easy.txt 1565371553_estimate.txt @endcode @image html eval/plot_xy.png width=70% diff --git a/ov_eval/src/format_converter.cpp b/ov_eval/src/format_converter.cpp index b69be4980..4cf9c8bd2 100644 --- a/ov_eval/src/format_converter.cpp +++ b/ov_eval/src/format_converter.cpp @@ -134,8 +134,8 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 2) { PRINT_ERROR(RED "ERROR: Please specify a file to convert\n" RESET); - PRINT_ERROR(RED "ERROR: ./format_convert \n" RESET); + PRINT_ERROR(RED "ERROR: ./format_converter \n" RESET); std::exit(EXIT_FAILURE); } From 9d499dbc2f87bb8ef115e7cf40ac39f6376202ca Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 9 May 2023 17:39:39 -0400 Subject: [PATCH 41/60] fix #332 chance reject problem, fix index bug in closest feat map for right image, fix mask updates for point extraction --- ov_core/src/track/TrackKLT.cpp | 45 +++++++++++++++++----------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp index 21cc0325c..d2fe388f1 100644 --- a/ov_core/src/track/TrackKLT.cpp +++ b/ov_core/src/track/TrackKLT.cpp @@ -457,7 +457,7 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, if (x - min_px_dist >= 0 && x + min_px_dist < img0pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img0pyr.at(0).rows) { cv::Point pt1(x - min_px_dist, y - min_px_dist); cv::Point pt2(x + min_px_dist, y + min_px_dist); - cv::rectangle(mask0_updated, pt1, pt2, cv::Scalar(255)); + cv::rectangle(mask0_updated, pt1, pt2, cv::Scalar(255), -1); } it0++; it1++; @@ -593,7 +593,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con if (x - min_px_dist >= 0 && x + min_px_dist < img0pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img0pyr.at(0).rows) { cv::Point pt1(x - min_px_dist, y - min_px_dist); cv::Point pt2(x + min_px_dist, y + min_px_dist); - cv::rectangle(mask0_updated, pt1, pt2, cv::Scalar(255)); + cv::rectangle(mask0_updated, pt1, pt2, cv::Scalar(255), -1); } it0++; it1++; @@ -662,8 +662,8 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con if (!pts0_new.empty()) { // Do our KLT tracking from the left to the right frame of reference - // Note: we have a pretty big window size here since our projection might be bad - // Note: but this might cause failure in cases of repeated textures (eg. checkerboard) + // NOTE: we have a pretty big window size here since our projection might be bad + // NOTE: but this might cause failure in cases of repeated textures (eg. checkerboard) std::vector mask; // perform_matching(img0pyr, img1pyr, kpts0_new, kpts1_new, cam_id_left, cam_id_right, mask); std::vector error; @@ -674,22 +674,18 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // Loop through and record only ones that are valid for (size_t i = 0; i < pts0_new.size(); i++) { - // Check that it is in bounds - if ((int)pts0_new.at(i).x < 0 || (int)pts0_new.at(i).x >= img0pyr.at(0).cols || (int)pts0_new.at(i).y < 0 || - (int)pts0_new.at(i).y >= img0pyr.at(0).rows) { - continue; - } - if ((int)pts1_new.at(i).x < 0 || (int)pts1_new.at(i).x >= img1pyr.at(0).cols || (int)pts1_new.at(i).y < 0 || - (int)pts1_new.at(i).y >= img1pyr.at(0).rows) { - continue; - } + // Check to see if the feature is out of bounds (oob) in either image + bool oob_left = ((int)pts0_new.at(i).x < 0 || (int)pts0_new.at(i).x >= img0pyr.at(0).cols || (int)pts0_new.at(i).y < 0 || + (int)pts0_new.at(i).y >= img0pyr.at(0).rows); + bool oob_right = ((int)pts1_new.at(i).x < 0 || (int)pts1_new.at(i).x >= img1pyr.at(0).cols || (int)pts1_new.at(i).y < 0 || + (int)pts1_new.at(i).y >= img1pyr.at(0).rows); // Check to see if it there is already a feature in the right image at this location // 1) If this is not already in the right image, then we should treat it as a stereo // 2) Otherwise we will treat this as just a monocular track of the feature // TODO: we should check to see if we can combine this new feature and the one in the right // TODO: seems if reject features which overlay with right features already we have very poor tracking perf - if (mask[i] == 1) { + if (!oob_left && !oob_right && mask[i] == 1) { // update the uv coordinates kpts0_new.at(i).pt = pts0_new.at(i); kpts1_new.at(i).pt = pts1_new.at(i); @@ -700,7 +696,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con size_t temp = ++currid; ids0.push_back(temp); ids1.push_back(temp); - } else { + } else if (!oob_left) { // update the uv coordinates kpts0_new.at(i).pt = pts0_new.at(i); // append the new uv coordinate @@ -722,6 +718,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con float size_y1 = (float)img1pyr.at(0).rows / (float)grid_y; cv::Size size_grid1(grid_x, grid_y); // width x height cv::Mat grid_2d_grid1 = cv::Mat::zeros(size_grid1, CV_8UC1); + cv::Mat mask1_updated = mask0.clone(); it0 = pts1.begin(); it1 = ids1.begin(); while (it0 != pts1.end()) { @@ -751,17 +748,15 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con it1 = ids1.erase(it1); continue; } - // Check if this is a stereo point - bool is_stereo = (std::find(ids0.begin(), ids0.end(), *it1) != ids0.end()); // Check if this keypoint is near another point // NOTE: if it is *not* a stereo point, then we will not delete the feature // NOTE: this means we might have a mono and stereo feature near each other, but that is ok - if (grid_2d_close1.at(y_grid, x_grid) > 127 && !is_stereo) { + bool is_stereo = (std::find(ids0.begin(), ids0.end(), *it1) != ids0.end()); + if (grid_2d_close1.at(y_close, x_close) > 127 && !is_stereo) { it0 = pts1.erase(it0); it1 = ids1.erase(it1); continue; } - // Now check if it is in a mask area or not // NOTE: mask has max value of 255 (white) if it should be if (mask1.at(y, x) > 127) { @@ -770,10 +765,16 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con continue; } // Else we are good, move forward to the next point - grid_2d_close1.at(y_grid, x_grid) = 255; + grid_2d_close1.at(y_close, x_close) = 255; if (grid_2d_grid1.at(y_grid, x_grid) < 255) { grid_2d_grid1.at(y_grid, x_grid) += 1; } + // Append this to the local mask of the image + if (x - min_px_dist >= 0 && x + min_px_dist < img1pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img1pyr.at(0).rows) { + cv::Point pt1(x - min_px_dist, y - min_px_dist); + cv::Point pt2(x + min_px_dist, y + min_px_dist); + cv::rectangle(mask1_updated, pt1, pt2, cv::Scalar(255), -1); + } it0++; it1++; } @@ -786,7 +787,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // This is old extraction code that would extract from the whole image // This can be slow as this will recompute extractions for grid areas that we have max features already // std::vector pts1_ext; - // Grider_FAST::perform_griding(img1pyr.at(0), mask1, pts1_ext, num_features, grid_x, grid_y, threshold, true); + // Grider_FAST::perform_griding(img1pyr.at(0), mask1_updated, pts1_ext, num_features, grid_x, grid_y, threshold, true); // We also check a downsampled mask such that we don't extract in areas where it is all masked! cv::Mat mask1_grid; @@ -804,7 +805,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con } } std::vector pts1_ext; - Grider_GRID::perform_griding(img1pyr.at(0), mask1, valid_locs, pts1_ext, num_features, grid_x, grid_y, threshold, true); + Grider_GRID::perform_griding(img1pyr.at(0), mask1_updated, valid_locs, pts1_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature for (auto &kpt : pts1_ext) { From a824660cecb4fa442ef3ea849c2b6deb32ff5c4d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 9 May 2023 19:20:18 -0400 Subject: [PATCH 42/60] documentation improvments --- docs/dev-docs.dox | 11 +- docs/dev-index.dox | 6 +- docs/dev-ros1-to-ros2.dox | 11 +- docs/img/hxorder.ipe | 318 +++++++++++++++++++++++++ docs/img/hxorder.png | Bin 0 -> 34007 bytes docs/update-delay.dox | 6 + ov_core/src/cam/CamBase.h | 5 +- ov_core/src/utils/opencv_lambda_body.h | 2 +- ov_core/src/utils/opencv_yaml_parse.h | 7 +- ov_core/src/utils/quat_ops.h | 7 +- 10 files changed, 354 insertions(+), 19 deletions(-) create mode 100644 docs/img/hxorder.ipe create mode 100644 docs/img/hxorder.png diff --git a/docs/dev-docs.dox b/docs/dev-docs.dox index cee9ad98a..29f683149 100644 --- a/docs/dev-docs.dox +++ b/docs/dev-docs.dox @@ -53,22 +53,23 @@ 2. You will need to install python3.6 - `sudo add-apt-repository ppa:deadsnakes/ppa` - `sudo apt-get update` - - `sudo apt-get install python3.6` - - `curl https://bootstrap.pypa.io/get-pip.py | sudo python3.6` + - `sudo apt-get install python3.6 python3.6-distutils` + - `curl https://bootstrap.pypa.io/pip/3.6/get-pip.py | sudo python3.6` - `sudo -H pip3.6 install jinja2 Pygments` - `sudo apt install texlive-base texlive-latex-extra texlive-fonts-extra texlive-fonts-recommended` 3. To use the bibtex citing you need to have - - bibtex executable in search path - - perl executable in search path + - `bibtex` and `perl` executable in search path + - These should be installed through the texlive installation - http://www.doxygen.nl/manual/commands.html#cmdcite 4. Go into the documentation folder and build - `cd m.css/documentation/` - `python3.6 doxygen.py ` + - `python3.6 doxygen.py ~/catkin_ws/src/open_vins/Doxyfile-mcss` 5. If you run into errors, ensure your python path is set to use the 3.6 libraries - ` export PYTHONPATH=/usr/local/lib/python3.6/dist-packages/` 6. You might need to comment out the `enable_async=True` flag in the doxygen.py file 7. This should then build the documentation website -8. Open the html page `/open_vins/doxgen_generated/html/index.html` to view documentation +8. Open the html page `~/catkin_ws/src/open_vins/doxgen_generated/html/index.html` to view documentation @section developers-theme Custom m.css Theme diff --git a/docs/dev-index.dox b/docs/dev-index.dox index 7472c7a72..3a405f26e 100644 --- a/docs/dev-index.dox +++ b/docs/dev-index.dox @@ -36,14 +36,16 @@ The error state needs to be a vector and thus a type will need to define the map To show the power of this type-based indexing system, we will go through how we compute the EKF update. The specific method we will be looking at is the @ref ov_msckf::StateHelper::EKFUpdate() which takes in the state, vector of types, Jacobian, residual, and measurement covariance. -As compared to passing a Jacobian matrix that is the full size of state wide, we instead leverage this type system by passing a "small" Jacobian that has only the state elements that the measurements are a function of. +As compared to passing a "big" Jacobian matrix that is the full size of state wide, we instead leverage this type system by passing a "small" Jacobian that has only the state elements that the measurements are a function of. + +@image html hxorder.png width=40% For example, if we have a global 3D SLAM feature update (implemented in @ref ov_msckf::UpdaterSLAM) our Jacobian will only be a function of the newest clone and the feature. This means that our Jacobian is only of size 9 as compared to the size our state. In addition to the matrix containing the Jacobian elements, we need to know what order this Jacobian is in, thus we pass a vector of types which correspond to the state elements our Jacobian is a function of. Thus in our example, it would contain two types: our newest clone @ref ov_type::PoseJPL and current landmark feature @ref ov_type::Landmark with our Jacobian being the type size of the pose plus the type size of the landmark in width. - +In the above figure the orientation and position would correspond to the x1 and x2 states, while the feature is the x5 state. diff --git a/docs/dev-ros1-to-ros2.dox b/docs/dev-ros1-to-ros2.dox index 6589faaf0..7c7978e43 100644 --- a/docs/dev-ros1-to-ros2.dox +++ b/docs/dev-ros1-to-ros2.dox @@ -13,11 +13,17 @@ This is what was used to generate the converted ROS2 bag files for standard data To do a conversion of a bag file we can do the following: @code{.shell-session} -pip install rosbags -rosbags-convert V1_01_easy.bag +pip3 install rosbags>=0.9.11 +rosbags-convert V1_01_easy.bag --dst @endcode +If you need to create a bag from images, then we recommend using the [Kalibr bagcreater](https://github.com/ethz-asl/kalibr/wiki/bag-format#bagcreater) utility. +To convert a ROS2 bag recorded to a ROS1 bag one which can then be processed by Kalibr or a ROS1 codebase, the following can be run: +@code{.shell-session} +pip3 install rosbags>=0.9.12 +rosbags-convert --dst calib_01.bag --exclude-topic +@endcode @section dev-ros1-to-ros2-option-2 rosbag2 play @@ -42,6 +48,7 @@ source_ros2 ros2 bag play -s rosbag_v2 V1_01_easy.bag @endcode +We don't recommend this method and instead suggest using the [rosbags](https://gitlab.com/ternaris/rosbags) utility as standalone. diff --git a/docs/img/hxorder.ipe b/docs/img/hxorder.ipe new file mode 100644 index 000000000..8f9145a4d --- /dev/null +++ b/docs/img/hxorder.ipe @@ -0,0 +1,318 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +128 704 m +128 608 l +448 608 l +448 704 l +h + + +160 704 m +160 664 l + + +192 704 m +192 664 l + + +224 704 m +224 664 l + + +240 704 m +240 664 l + +x_1 +x_2 + +x_3 +x_4 + +x_5 +\mathbf{H}_1 +\mathbf{H}_{big} +\mathbf{H}_2 +\mathbf{H}_5 +\mathbf{0} +\mathbf{0} + +128 704 m +128 608 l +448 608 l +448 704 l +h + + +160 704 m +160 664 l + + +192 704 m +192 664 l + +x_5 +x_2 + +x_1 +\mathbf{H}_{small} +\mathrm{Hx\_order} +\mathbf{H}_1 +\mathbf{H}_2 +\mathbf{H}_5 + + diff --git a/docs/img/hxorder.png b/docs/img/hxorder.png new file mode 100644 index 0000000000000000000000000000000000000000..1f5b9cfac410df58d1f00a69e53094cd06625b1a GIT binary patch literal 34007 zcmeFZcTm+^mo2)@98f_~5RoK7KoAfR0RcsV1j&+vk`WOlN;aV=AVGp4K_p3*B$7=a zL2}LpBnQbE?%3yi-PK)vU)8Jo$9r|}ZMwgMrJK!fueIi!V~#QAdZMBvyN~oJDS<%P zCwECojX>Bjg#Xc!Y{%ay?>|z2Z@Y~ZWTgmO#Q%F$n*5wVU?j*%UC_7_HPPp+e`vUW zYfC4RosopI^KHc=Rwva%dWR;so%Xd`8ns2UJwXrltctde?f~?wQ@|i?;6Sm?LK>&h7Z--b?dH$licIUL{52Amh6N z+ks5OQrE4`4RZQ(RrU2@#mDeyly;7eHho14Asp&Oj^jx&uNiYL-a2sLz_xAM8e%2F ziz)D$+|}7yo(B&e946frL?`LCwdwJz(w{-xo!yxU|9VMfd8$c(P)| zap%MM``KHLjvfnRq5i#QgwVt#9yeeETvFdyqS5k4F{nV*b z!7NwOwS(e!=0%G-D+MR0CCX=;TnZi>9AuGG`&|>da>g{FpLCUvcQ-x;#nB+T|Ni!W z%o0SFRf&6Tx_f6!%g78BWmZ*PiqEp%Twh6AX~r%S=6-OA;v1vD@qfOFipB2Qy}PKe z@b_xdw+Q|P9_O~yD}_qVj*fNKu7S9S_7_;8;WC(7>$+0r&`+GlR&=S}yJKKM*Ym??Fc8>$LowjZva?9R7- zuzMfpl~j#Xjg(+k1W>)>-&d$JLp86_U_-mI5WfN z*4F;B?xhg*DgF2ED|UD8+|kg`FeZMzmQUnVPg0CHZB|lJqMK*glzJu2X|g+%TTgS! znC$Ma7#fPMd!N4ld02Amhq!4TJ~L1G`e|n7i*CkUIcDV>i#<*8vc<*4)@)^-8?NK+ z8MWa&%1TP@is$Ty6p^*IHkZo1W-6$MPIikNn)W3B4=dR8?WNH4+H`qGL}!j!?ZwCY zue?#YU{REuJX|o~Nqgu}YnoP;#MZhxzZgINTrE$z`|AAY)59kf4YM6z4eXHl_&b?E z&@Kr*sWtR%Jm0!Q+;L2!z9dsMPSQKqqMdqj@Q{>GUYX(ooo`227~ z>_CZ2?kj~{-FdeUQhzT(!so~2!wafR^v=_hlhGZr`U{q4|NU)(V^|D)sxpVaoD1qZ zj~{XP`$uA&r~3^`T?Mn0$qsZWgrCAsX=NE+mXqTc*`7gxMWFk85l$RGE}5wId%=on z`|FrB?=+3nwWVolhkKen_$2#@pQJWk;v%h3&Tz5FCA|5Uzu%mGgGN`myH=q(n03M3 z6`^?lRAYDJ^tR`xub65b33$S}lkmY#T1skUMPT^X)Lfz$)kfL_;!^tM68AKwvr}6v zw!8Dt@>20;6+HukQR6OMT6`e2zt=@sO)WS^ne29$fmcmO`9HsQLNQNY>~kS$&4`J~ zX?}inHMJXCJPW!<9`7K$C0XUqYih`$?3$Ctuc#TT&%wuVT^K7w)D;zph>C`D=`?@* z__4jcy|Qws`uPbvtv!Uh7yLZK47RwQJbALq;8lLyzz#Mu#~?9L(PG{F>j;^5xoZ+) zdk=GJRE<}EcvO7*mkdI;ueaB{A;#c~^tQK>`tQj>rBe5m(_&&9;YOZKinaq~9tXmYQB!v$DuhdHE;9=YyHq?qjO54u zbGxek?K|659OM(#8j@usl3zwg8~odvXi|3x-neliZ%=OlPMHtcIfs$J#Sb}>>$kQO zt}<@Wbo*$9(B%BHOp1;xDbnxlem@<`FE95x5T4}WKEJfYv&u_LXqOozTVgu8;ih12 zp7h|s#d}mQA|iA~+Ah2vh<|%_il)l7Ax9zDVP)2Afy~)?3Al;+wiPhQL|1OIudf0? z+S2rZ(cqZMjTArGsV!1M*GPl?>YOu^dEEF}qj|SO3Pk+XsQg?l#eHcinu9aDKX-yN zRNg+U6u~9ERhw>F9V{C_&uwQwU=&GIqoJa5`t$vxFEx*hv@|O#D-vpZOH1yRG)+{6 zsiyeHo3bB1u-~w;S%_+9&3VbNL0_SE{`?1Q%~3$2be&wJEMk`W{P}Ym@#{*8M&;M9 zUn?q7wn}mK5mH(Dg{QB6xXZ2g?nvpuasLTql6HL#wRu&(^D|U5G{TmRuhP@GE)N_^ zZ(o@kF5g@oAMeOg2-YZbUx^g7W;=00!edo{lXDD9f$cIWwELZe*th8xkd>7!^yRLf zn3$NC5{Z{}itb5gIzdZIyWu58Ba|~I8{+b%q9QRl`SIUhw9REWj0izNY1Vd8-l0$4 zWRV?GPK-R$1La1=PB!r}{-+E}EWd9ftLPkSOA~dPSe_qk@g}3LtgJ-ISp8Yy>payf zcj;1RnMZL!K>>^0?c2BQhrU^Lho8zAR>yN7$U=-BB@KK&Dsa7ZWNE-_ zX?FH>#iu6|!Ox$oKWDo<$i16F-rRgBUN&HHqKp65_wx%hEkd^am*zj~mYUUu)kO&t z*Izm2oX*>;7w~xn7(%&p>;|iX@FF19T-O?;EF?mY;d;7=r`(&py}k49*XQrP@p25D z9rt-P5Z_6Q!iv>AsqyCO>)(`JiCC|<13Q9Q#N3zPYG&wS`%dZPm}cnaKiW^{y8Pz{ z&~c3c^}|~zU8o2vYio}mJqqJcXX`kVnVGqVoPK_0Mz#F1ygcdNy@T0fY{4CxD$IYM z2F==mfvv&n=WC0T_KuE%eY)uasbzO&uv*k(9ez)@wVpnG`t|GAH6a|oe*J2Bd+jrx zWbYB)54E+{2t>Q^-4Pt2HCg4G-TnQZc#rz}dRN_|_wUp73Z|Ybi>sESQD|FVnY$<{ z`Asd!ktxTwvaW7&VZ6h|qlPNm(rs}<7-a@k^>2`1eC*heA$=t-b2w8}GpeTOqPAkUR$>^A8*RWABB93E^$r+WNQlGQqRoP}m)F*G`Icwns|)@8{m43sa&j(6fRd7* z0+}wd$cX@<;r;u|y|Qv2qK&$H_pXnR&lAcMh)fw-*)%UER9HuJR=YBWaT5KH3U-&e zIXgKyVZRYX5m8ac4j*2?Awm>ijuu%!87TDH^287Hj|ts;VjxPgP5N`sAjdK@<@Sw5#x1XoXSHQ@Wav>-Qzx&CAd4fXj`b8wV|7v@-dIr_IB@L_E8>er%r$TIn$_n= z`+D-Mo|l%EI_;pPeTnxG1F%EHj&Y!C7O^3GXkA^J&KkB+inLHjn>nIEIogt}Y{+o< zc+Pl)bi>nE7l@#sn|X~`$dqGatoavooaBO$9h@dQ<06O|77fYG?mW5T8Gl-#RvyIh zKCWL_m^6ym=4NL;K#Hz#A4A7?@SITGQ9p z_xZC4(%9DY)`rB!B&y+?jd$)Vvq|aot_zbrLRkUxQKxynaJg#k+(*PZsOv2)ErR3* zz)x$lzia2Nr))exrWaoDMNDH?_R*hvf1@j>+aD2?<$3LD!seeJpYi{K9l4{s79{Mv>bZaaenf}zrAs~m0SG|HG7k@tpgus_kTxA2ob-|@HRCV26E|<( zTp!~D0<7roD|R0K7SV#;w)s)p z4{REFZ6tbTBVXW|lmW|&&Ft)Q%t_WPVm-_+(U4Jby?36DS#`wr^t`i4OP+atLRlb7 z_~v$sfv#MO{DIxOch_iK;#2HQgAVC7QRTwQxWqm?gu_^^JenD^DxJm7&Q83kWxpp{t<@f+6~m3!XbvBdmuw^Gu&}UL4*2x- z^sKcgON3^}{Io%rOX_`mcpiY|x8w7l`lUD~GrztHC!aj@G=krPs!aSOz~1f9z`*ar z!_>!)^#ap1Ha3QYgp7=g;CBJV^XR{C?C%#pe*9BIgR*EJO>|ju?B?d$wAcEK?2oWw z#Dsn_mXN9Au|aY|!j0bdP4CBN7Zw)g=2{}&&{R#0kEhv_@1*MvGO(}oK&2Fx4cJ(p zYZ#+O3sk$%7?FFm&Yu~lZo#N(VfME|osN^I)HWYmEPZ6XqFgW)x9%5V-If%ia!+74 zd+~H&%V=>A-L(kK)#!6Br#nVHR_FN|K6m==aV-CeZqVLSs{FCEy2P7dWNQ(RM@R?Q z&ym&?b>zV2mX;IBF%u&rXPqXrpC5{==|Mg5D=WeQ`CXKW|I&CEprLEg@l{J{U0*?Z zmK57^4wcV0t{|;@eZ4(BLW{rdV&UZEB%UY3D}pnW6DWR7`Ome_kC0IcwftD*kbu8A zuT*@KW$;MwI*F*zn>yJnwZFtgeM)y$b$!u4s=2ut%vhX`LY85v zV&3tKZ;HjJxpi4g_L=|rSwTik9Uk@ay~o<(Bc24LO5yPDgM;(HJkJlGqFsn6BW4}r z9{_~)3xm;;ahEP#;&m5x>ANNqr*dRTJK1yhdQIp_|M!YEL+7X>j19(mCf(fJBs|x( zmHF#d9QE{i{`~oKKU3ObSw@N2SP1eCq2{BN80%Px$!?#z_C_U6(y&e-xf30+O_jj` z&#evT%lm>^7Dk#bdbv$9#YLIzVSJC8Z6&hX!`XO@FRHj;rQmnYmvfaoM#lydekzb%3UVEHAo{ZX*N^5Fi z5kM<+>g37Vkx*?d({lHfoK+Jgj7DlWYO_vg zz;dg(x-IVFW4lR>t)IR;c?=TUKWFIVvUPX{Y~SZ#Snlaj|3pbiY4;}0Zj%V!YoE$D z7qZ;96mzn(X+#{}tS%s@EM$3Y;AuyEC(sX^k|MwF8OnL3#?b)(J-u2)#bcPBOI24< zaye3<$zEMrE*SYBZG+@N$xUa_INKH|FQRvD&bQVQBWrCGfBxgwulTJcnjKx+w4bJ@ zr{DfnnIfAFs1q&Xm}*?{)Q;Lrx;#T7i>Z3)6 zyA}C4&A+|iD-MD+n`Et)Uo;xZRoN)IpK}e_|$_BYLVROUO#Dw2<-g1X^qqvK*X@UN+n80xCk0UyL z@0UVZ#iRRZPP24)pm!~76#8CkU5Vg2Y&_5jAdH@gMQ(O}etn_Ch{pP4xUl_@!)WvG zw5+n`LTLwW;@1m*fB*3CaLG8ICr?x*-1KCTGSI^{#Ao>#gZ-(luI^q63?$M)Um3iP z@tZ4V5_bj5Rq=CGEPd{M@T&c|PN^9`v!-&1e0<3LKeis7!3TG&b zT&(--6D8TUGLN-`l$4G!?-P<-ex|U<0l#+`y=BYMN>NKhkyxeu9Ch~GwxdXJYDVAG z<7M{MXVqp(`%)oCfBo_KG1?yYpI`o(w9M4tv@dNd$jGok3koEPBx`*&fNxg1{Bizm z2ZvVAH3$eE8w(vW`enog4mvqp+Lmt>@Dxp&S!QwpXvYGZo+Qr<`yEP&YFL?XB{7Oa zQ&XA3#zTTRMwi_fMUy+X`EVRR{u;Ry(803xO%noafx+M!J*oF=B6qO1=z+6}-U@x~ zEbs^r=m}QVl&o?)d*avJ>@3o(4lww}#zxmIHp%kG3yn`E&@=E(&1Rg;3FlCk5j&Hl zk)r03U;XVH)-8E8-`~HEc&Kcg@F^E4sL}U}d#qY|9ipOI1R^#|z|=f-QUC@Lymy?RwrQWDC?GkUSHu`x~)zN+e$7JD$P zk0ez7O1iplgI?3Ix7N3QV;Rkuf3f@c(+QjyP1!|&T(H7}-R1zrk&n+PN1RRs@+myi zGB}FLkm8mv@a|@xE_MsG`nQP5g=+ft@p z!OeykF?1yz#13fS5!qYwc{lASRIZ)7b{Uj=mZG}_+HCexcghDxU{&6>xAF`X9tHt; z#fWzm5K*6bjfh!xgdi=KZEdcVP$iE37z1=%oCkY}lje*lb#f~7Dv0%1?0PNj+v;U1 z;X2RW@$y_!V&c9dyyxxXQp3Zw(BaN*3~amGWomA|I6bgc=}!yNvK!Klq3k)i;Qe$W zc_v@?Pu{%v;d;~bH4Zk#7q@C-2?l$wevY?=%3ZA3Zo?cIJV=A(SZ4_7OI zr6cEB-oJm3V0B;qb0L_;)^-|?J4Pch2MDm>yL;bJOE!?~I67z${TL*~vuM(;q@B6m z%D!Mdxz=o1;m!YE*um!K9e`0Tvx5N9b(*w< zL(xM&UIw#xKYmPmx)wTA{=kr6WPD@#$()WfEoBdh_3z)mgU*`_Ff6%qfP&%{q&T@? zPK`GdVBoh@<0D^|o;`c^jS;aX(TOgi7aH(bir(Dg0|G~8<7^ucHwuKoT09x`DS?~4 z%fRkia}rze5tmH&Fopm#PUD~g{D2jCo1fn{uneGr#++aI8sX}pZsrXT^4F@kx`k*% zHm9G5`h6-oUd$qgk__;Im;st1HVbxkv*@|Qx%Gxn7?c`tpo>7oM&(4#;m)5|NaT<58mg)~ zOWg{FGC(aN+xqDRgXfeB1`0bsPhW=R219}L(c0Bjhz{S=(-Q~2+0U*T`M$HwFh6=RRQc`b>*4Ea>d(V&%Om9#176viP zt}ZXnk9SN1mKMHKXSSexgFEZ7&v@rM zo0mhGOpA+)OiWBNJk>{;aWc@Gqn|wK8e7o_z_Oq2dPr~{6dK#ej=iRjOB`ZeW~iks z(>9Crfoj7C0Y%9g=zqSg|D)A?co-L#hV5 zyO)H-$Jh5cO9~oQ^nLxV+l~Is3lNK*@5$5b=6ig{ef|7!hLTnt&!4|5eJ9-li`^i+X=;WQ>5Mifo&exOmjdxCGV{ps;s`v1 zIgUR_sl`8kzikZz~$GSJdD0+7Lt)#`To6o*9V^~f73e7CW-Csj9ViJUI1@ ziGyRb!^o?x#L?NgAzw#I>I-p;e^GF1Wv(}PZMK~IO*9smN;%n|)X5n{y|q`8up7LD zhs;a75Sy`EZYgbS5>&-vU2qce;#3~>12AXDXeiaYsHZd^}WYB7^ zOzEbFyPh1eqBwR;5f+O`JUSrdoAcqGCEGGJ(T+rDy_Vm20+8@{UMwKXFS3aVH zB}h9O)U$2p+Y3Ku)qQN|N1DplXB7Yp22UZNKPXO2PD-pz;t1Sg(c}zu zjl`+FZ12AE<1xc2qp}+&ChE{oqJI4`uaDx`Famv&w9fE><-feib0Dj77CGV%D=4%K z*nNFQ7x_y87)R7$gi%h-($ca%3|Ls?Ydj8UM3VapQqkR{59wqmZjk)?xyH5g<}Aq( zBj961u6zi4HZadYR{0TI{Y-t~x^dJpx!_i`EDPsLqkP*UPMcN*9;YUo`%lNh`*VB< z;ny07O;AxN*a?LsG687n#}!dMr82qEGh}VFcXT+z(&l~Y;Lz7+_qJbqd|)T-r}xX^ zWO7q2>*sG%GB8ZzP>hYGmZ`8Ap%rM|403RAP*hT?&o@JV87uA)6&;O^4C;Hdt=I8~ zpZ?REAlU8(5xG%dLL-lbP{^)KlMLW9Hs0ajgPE<*hZ;K!#OJ4SeDK3RKl?E8hR?Te z-%#+e{U^0Dk61?}F;*?D0Q`!Iih}Oj(Q`7irnObWg&|embdTQzOf11Hu;cRsY2I9UIb*-(PPTx2 zAOembn-1dosIxyiMU&GJWE3n8NVpAXLOg~gpd-;IJbe6E76{ti!vi3Ey-AjV#@YWq zh0Vo5GoU%t@9gXY9v^ZU2Q2=VRdKUPZJ7aPH34 zr=Qf+)jQI4eoalCB0^E#3eH!LEk!HAxFB?18``bR{>BlRL{k;rR2zD-09^v0{E^#< zyit~--yjyEHMj;)lpyyU+&K?$84e~g4%)3^$vBm)Y>47OrMy4or5U>)Rn`8JqAD63 zf4ava#|jFtD+II1fddVA_UueeD%s_c%VgS7Z{ECtlzU9Xq3XjT(nO0mJ4|KOJ{6{> zrr;`XA>Ly>SFJ!9=@#~zam-JL&gkZg#r z3{v`@E%BZ6pkmJvLW9l8S)-kHN+3>%TV=wZUQBoG{huz2*ux$XXrY!z;sd~Qm|9z> z8K#$(Zs5T9Oi+C}B>JcuIGk1PI{dBx!FD!*AtA(ot4oE|{s};^xJXY;Z$AUGE6>Em_?$g;;wPyhwJt-l5k4F590L z0P9(nw>^{h4hsr8aQHB*TyQ`@1N89w4;~nm+{s23e$7K;Zt|4wVsHYQ{)U%A4Fdxb z0s?txbPC_yc61a&0~inxuxg{GrUs_R-MvJW^zM@vmXH71w{K%*IF|YNakRDqXe0gL z5_<`g+BG17$O1^)kWGqIN#CAj>!xsp$bvxbYqbvYT`5<#+1%Vj2isd<8@p(fa9Zt+ z24|r)A-xsH4{1MB>^JmdIQRIP#sn@ZvC942V`R$K%?IoOW^f%M7%B(A$(SuXZ&s6; ziy*!qJ$!g5oAAPC^chR3+mg8J{F|Xk#H^#z$aVcfyF^ewkp;{$evO`%iJ`o6Oa?Zs zZ&aI_n!vVfdBX{v5!&YFm4&}9 zyqLQ5+P?VwLaDf2;+*2K=wf&6zvdX4KY#iJ;mjHE!q}ylp5Hz8Z_WSC+uPw*B76O| znNm_wC2hPz6|}puJ?@3&*JZR3%yP}ZFi2qMkVFuEBeoJScGQg{pYGVR|48EABhgq9 z5z#00S0=h0Zib@Wu}MG5|!LfQlgbe zoGfxR-@Xxd6H0*UJU}a0plU;my$_ z!yvad^wL(bh>B+Bq}rY!y1=h!$e5X#1#J(LN~8TAJh|sJZ{M@HGIkvk5O=FR<55Mm zE)Pt7J|xHnLsvzQ<_asOtE&}=TeoiA+1e}}%80qJ|GEfsM~3~I2g{1CEVt2{ zjk~JTArCcrih}7kX}ikt&E=9-ibky0<_b<4=+w)3q+K&7PoJpI*)iKVI(xEG*6@(e zsFNRfmvNVuFrx)8>eb~QW;%4?84Dh%rQ{~)Tb~2WrW_Y#hI%Q`db`8w1Y+oN0cqD? zIVNP@xf6HKIXsgE?--hPkfg#vR0c^OISbCxL>2b?_wUc|wNr6h{u8&@gT|0H?ck9h z5?P{^8w8|vKbmKmxnn}(*WT?rKu5PRSun7g=y!l>m0+h70X<`)( z4GkY$mL_|Khli0GMJZc%MMO9dmqQ!E3|L!t_jN!&2y+WODkpN*R#%T||Mg8KkhvP+ zB~ZK4Gq7py?Yzi4{OASb2*^d?wn#N!OY?0#^T7Lo$|eRr))^25>^g&hWg~KZsJz*| zFcaNbareV-mB3SUZ1Sb+=BM7{=M1fZP5^33L`hfFk>00!9-T!{o?*tf|@?c)4`C`_PH28qoXnhGif&}t1+2V|uJzUDFef`WsI)>3c@X}`?* z%-_+8GBJH@Xy7?}_C2Z^k_xa0vP*V$HtaS=?{CGyvn(a`FgA7o1&4Iqi+5XP&W<4} zcp&)Yv+(Y_>uHZ2?wFvrv@Jo2NAaIWy3&2MRnVyCFhX_ej*g>C@YQ;$(Yw=~6 zarTR%;?jPV^6~4}6XN3i+6ppdTxO4nUT~xm_|EdO$dJ>9;l1_E(Y{TR&jbvIcR7uY zrsSxP*B$DyHH{PvFw{;C4n+EE(;Rk?$T?~hpxf|JGLZ=#xFE!R^>$?dLzaCA2(Rj{ z8;Jg37GY6Q>w@)P0TQzYn~=|0|L!#6P9EBNVaWBQ++^g=pYIo*9As_X1vklMvE^;% zCbvZuVXs2(At?C1sEA(F>6}0(poOYt(NN@J%6C|&Wth23_>4mlL*0AiJ^$1xchWJl zu_fW)0B69!)d$7H@lllIsZUe9E`=Uz?^g9w9sw7_d#qS$sQ%pgqW=3^AK@@Xi9ph- zD#+eN*Ux?PYkf^kvu9uw1U?6cycDg-lgCA`a&wKm*q`eH({p+G7QzG09;l0k3!O(^ zQ3MAEJJ9E86gi9z4w@E@WjhSG&k-fpr)5znPI#8$ky;LVJo~{zha4d9f{{fzm0OFr z?IMQ(FY-}U`&+jfJx}KecU9+T67Ei*LBT3}p%7nquzRw%u%-S~!vG1eR5u1O&~km# zp&ty5j1~L|h96@NP#*dxV1M zP2*1w|HAl7APWZ<7u-mx^8cY=oTg$Q_TI$IZ^FbhwDXI>b(0cPDCV<{(I<<=_7a-9lrMx+d zahseW$2)i4Gv$uxdvg9Vw3+2QPatfoLxx5+1z>C-B57jSV__qbc}adE0=jLt=3=)- zY#A6BviPMfEJYbbC(B-;Bpf+i3y73I@Xn^Ey-JYA4|JTS1qI6g0A|@!f zfRK8$)D(ZGu~1b8O2a}yIRd;s1awntmEbmMVC-lxAt7kmpi;m=k;{~88T$f&c( z1Wy6kjN70{Agcs!gxs8*f5sq=Gc#i!W`I-CiUaa~_|_&V1Hg$9L*qpG&`2R$aad&F zWf-?w$8Q1#wGLSA!&w15RWxE_36)RW$W_d2VdmqTi4PDbdg}oP_gR(Wqro|%ztP>_ zzX?SeUdtpd6$Tvl!^S2iVq#)~wvh78N+#&+5I#JNp};NTMrYsR>XAXc!*eoqP6J-~6~)L2Z<7P~86UW8m@AV3~ztEi{|tZRBLja(3vsC3QD!lD5S3iB`y_7OhgFEDLtCM(f7 z$~u70ymMz+Vsj<{A24NU3_S-HhMSjHWh(W_#QEDAS%zYhC#;VUEt9=<1$XQn9JuB$ zu7RAylz=>*Um++ux)etSNe#GAUn~(^(A3lv;4hF!hk$ig&TkN2NQ1}KBQytbS^;ha z-_C%NgCJ+p?8?=OBZ_d*tDBaGI}}C~&^(yVSO@_>q6}1qmqB3)&HA>*K%7b%ya+d= znA;+e-{vtwRh$8Rl9QW@gL(2wDmE$^hwI(-Hu(!P$5M^BGp zjF;kP=qV^DjvRS0U3$6h$-k)kTIK&1ul`%O``=Tz`+xD&UDXBzmGm!_l_*^EfI*P4 zrgRAeZ!I8lNVra;%@ytKnp3)dQ3v<@n-_q9ywAcJdL5M#2}em;8KdfiySLG6I3t;H z>*fU(Q)r9s8s5Gw9?pPDa*1_D;>10(y`9}E*a^h%ac8Et*I^$cLBrOXbeXmg-qIIR zfH9!il2q6sxV8Ht@R&Q{_3vdR5EOr-=s=dY)7qAXa_dLLz;@vf+u}ec)EaD0$NW0) zebDoT3V5SNXj%{eHU0k;!vE86$+6pqzK@UVCCVQzOuHL}e7&DRoJ&p(Mp%6PKDB4N zu-oDdn0|o6pfwC;R=B1V-0!_Ya5;`vofn{o>Efd*gI$9mFJ6ogIq_xyxua)pusaJ= zTp~KtqD6(!+JQQ(HSk>+>&*TIOlMCXE-I%zv5AH{YyG-zh4? z_d#Z7`Zi)&`}b=ZBBOhE5qKZNivsAPFA*thpS0-$Q(@@1gO*lDNz$;OLR`mhj0kwi z9NPb{cesa1-fDv#S7$&-6FYgaeNNN;Vy0CTxozrTB(4RaF+*hp78D4x1j9d zjN;?upHp^kVX1TpZ5+RR;g7l3kU-*{kmNWt<4?QE1$@=3A7B z_-}4#pV#^?07#;f9{dKY9H&v4JKB6A7!~(8diby-QW|KhZ1A*DAz*=xgjm~KA*qFI zhYJ?Ku@f7xNz5;n0T`=m?W(#@^wb&tr(sdY?QUqmO7ZWZPRGfIor3H(?AQ0D=H*|ZRcN|?r8eVTtM1xiHxlvym{;^0 zfY!6%AbU$(_`!+6j$c=Inc^Y1Nw_A7qpQ&L&N!B_2eDm_j1qHe2Vd9vgdh{wm^RT% zF?YSTzFz6_Wx*|@pQ4GsHaHDWmVAQKsRy405(iwhy}kMuD7d(}cWmDdj!iN8sG=>} z6?AGhdfrU}oTq7|9QR7W1Ob}TX|yXKO<-zeBl(0iM}y)v^o<7nDCKY8gEYKXKY-`P zRH$M!$yF|WGg&L&Cr?rgON+4XFwJ1R8>8PD>FHlqMfv!e-e&x{URzW1^vRP0J3sdH z5S@`|Dc#^lfsa!$np$xhEiXcGac)l3ZqP@w^wjNJw>onz0x%Bzesz7t9^+kT5Y!YY zcVisM8NRK+K#DQPlI}yUp>$bqF#~}xX9LBJIcru{xgJ!vsHmvDJ)~J@-~`Rf6HvF< zR-u~KMm!>kiDtKMy^W5Jes;;tZ4HDHI`7N+9zfWzA(kN%sL4^TqK{r9^2bJHm}keA zg_+qExaY~qiG1ul8b~nONlFy#`$8tzZO{iav(TfdgPMIX92PapKbnM^ICKDg?{Awx z`9@{s5O@z(21m9Sbin%}G)`zHT<+XCA}o98Bh=D_2KQIJ##i?$LL-rHwnKOB*G+m} zINNRM<^>AOhv-A%1r1?gYC7_af%od&yr`EiX^$Sg-n&kCHNxI?O&(C9IR{BeRc{;n zi7x>@kjK!4()8Mz!n40U2hm&_5tOfA0m~ZZ@cVY~14#ETwlc z2q6q0F7mRoyTJ3xCt?U%AG4iP%YZvIS~NHJoWF<9cDgyxL0MTD+YtZmpI;A}0R2~@ zYo&GQ1e!lSNi|;L;_ z_HhhBLheHcZDL}ANGDo?e}^04#tj)6nbp-*9JSuoi0v|8t~kAT`4ihf+v2s6ybObFP#EBgr8{>=Wt^N5zYEhASPTWwhk`jefW32u|e> zZ{FA3JaRpM71m-Xp*O&`qdU&X$iN7}@bt6<=qluvv72rffd{&SrM?7KC5Yt83JSHf zl(<6xM;G~2L_|ar=ov>A`fmv+%QCv8g-(_re5mq3;5^r9~>&BA+4p0s~a7PToiJJ3j zn*xmF%kYfsy3lAT04!aMCxptMBg9-Y@og5k5twlm$KHa2B8K6Y5AbQhU&Hlg z65L0{OY^AcXw1tnl7JZXhYxr=rs{}}qs1h(6ry><)|O}S1_l@~(o^fEp4J9Z0(0P0 zJ5sU5g?45f*$8zoveXMBxGzf>!8{f1AOLsejbNcUy;+5*W6P( zUv1dzVbfpI_34u&#ZF|q@87SEQi6mNLWO2wkt$&_M{RWJ!QBzqUX(%oa!-Q^dBxF; zg8qX$;6yWYn-si2;kq=nfg5(L%EgFWr3SFE8UVoi?vv+?l}HMjeW>yWSUH^v{aq`z@; zM_VSrym4);qfjg;Cf&)9-@{t`deJOlP{AkIfN3olSRz@{o78vmYLdieRSz7>KQ661fk!9gL$AiOZq^asrcP!e05JZ0~EOHFMlCiNurr$7%il| z89o3`chtoJraA;caAztUG`jg^x*51+=_+_;BCV4$hbyT(T*C(X7XJLWC^pAonAUuu zI`fSO4adsC*QQ!kWl|{C%@62sAgB9^E&dpc1O%53f~jMld;?;?V&q;h9qHG}$c0*(NTg)n*HOlLrMv29Lx@;+59g=o8m zP5lCe=hns&W_6)IV2abd_M^~%yFIGVea2o$%LsF#M+=Q0=G9Qt=XN7+xPcn~Ig4&? zLAc1rhXmC&B;?Qq3K6FXj63(^%B7ke8k}wDP?0b=40_%c6xd>pI!?@5AQEZCr z^Q?$e^v*a$uT@i<1-E+N*mZt{X>tU=bT<6UOIhV;5mp-*boqpYs2|WBJC>f2L7e@#?+VLeh%21! z5Ixb&atOM zS+G)@&`gwB(dxoEHW(>X(ZD=Oy|WSiduF)|?!Anyk2SPMM}{p3H6=BMnVTk;Fy(su zOqObVCALj|ky+sGK|?d1SlicO%gc9MVsUu|=XG*@v&WzKOcZC3?k?$DqF=wjrUlUq7G|!#nXxjQlF5JmOjom1i<|DOik|D;J>d1bpP{_*0w}d<*j?>5fAzU zLYSt6yF*qVP(AzESLfP*Qos-IMXoAyj{`g78Uv~hzS~aV259)1{|I6dkxWE(PpT)` zebvT>+epk~H817#Hou3#rOOF+`s1~w`YI}ckob{=A!z6C4ujO42lL_VcHQ-p8C3%V zUI1kxi>E?c?=ym(94?+NYdL`1Ay~QHTwTX}4(+@K`nK8sEm-2BmO!K1M51LXMtb(A z-d16oFY53hi2k+t(dR)yO|a7e3xT_p@NWwiAXo zs~YM)97OmM{$L!ZR){z#uhsxDt-bGogIxcoPsSMcD0%??nwfjNjEZ<;2>`fnEr{zP zo@@(qb9td$r?FPKSSF#rXE{dIo=g5xXeK4Fz)v}X$i-9%QXcWjhXo5|_8R1c9Xobl zlCl}QixPRsK69KnH@apDAZYeMH* zBo4$>um@&+;f10OCqMT0eR8CSIR)Ue0J6oAw8c3Epdh<$ZDqw#zjVCNaXjp-mfx$d zB$WBO+-)_){w&Fus|t3R#0@CO>aqy}&Iybp7@UF(-E_bmst+X5+d2*RCZNSN{~IDB zCQ6hn;spW#tZ-`wPwzB(jnsuZEI44>6KHu52KfC%EAkoU|IEJ~8P$5RUzfG`*+BVu znhNd3pKISAkZ<3q0?ii*@6^a6DmcPO$iJ@&UpJ9$c7m@^N#I`!GpEbv2N#HEePT#Tt&! z!zY?|h?5>KSIuwyjvZ5|McHjVz@bFRkuheJ^N&q;d_UDrCRSGI)jy7oj%{<5S-{|g zCfuNel{xJ&(ui(Xj&hWW2x>9BlKA`x>2Zd4t(f@sh?J6&s?x$n*?x~AsPh|FZvy7f zx{s8mrCFD);TBTC<+3v1J;{KkZ{KdYR(3KGCw{BQeF*t9=2;}k<{V#N->Ko_xIIjcT5`T* z)&uX2yJBwZY%z^iSnnd-XM|nd^v5S}nCmdh=-oaJc?U8_*uZ`A=djVFtHj;+_rHv~ z(4J|KrcAlw?Q((Q(Ow$ks8+)BADqS8Uc7vX89L%UKhQ^z>fsiqfg~vKxf<*Q(Gyer z7}5u3^)&QP<#D!I3aC~0qZRA>T!$FpG$@Dr_g0zn_DfR(ZcN_A6u6P8V5x9PJl{jUK*>!V${U}Bfq|7#hd}=F=Zvevw4<7vTTT-DD z%!@*R#ZlPhr--wrR)vkh2I7GR4f2uqCfIjf&(|$WZL-9~N=op(zW|KwaN>|h;giC- z`z9olp*alk-NFLn0+^Zho%?Z`w{lSC*)d^+DsFB`eZ|q22avA1x?zS$6?=K?PT~>w zbJc0w;^Llgf8h=S3|4pHq&w`{LYe(Z-i0|u=Y;h$?WljF`+nbv-*Ay!pM$@?k%t2{$yJeT1}L&nPKFaK*RAR?j*#w7(|y&P=LtqvP?<`XaFbroueMC4g(}x$YX|SL07s|`mprrqvV;WK@GR!QFX{p+s z1HZ;0*Vy9qRD0BX2Pwc{1^d$B(l48R&I802%5$6A=KOp2>6B0~G+>o<^xKPOJ-|#5 zjYTe7yEA?nufTkZInvnaki`mXZ@X~-1hh!uwh@BXnz%nRW%i!jIdp+Y+1~w9Aw1;d z&6W?G`E0Ad^JZ^wZW+@o|`!w)hv916dAi2f1eVfcOn zZ5{0Ff`^E<#;|8GFv@cI7c?EMCJw2CExL4Zg>{y}pq~u~XQf_Tr$m!oLMDKI1NIN# zd&tO)mh+-tT9Bf!smGYvmGm>z3z zzZgI%(hLJ!K2Y6pzmRq78zvG-OgF&ftc9yk9mh3qbLoR=z_}Tjp0*uJ-)0hL%5$GM zfKiKU3%xxxV@0-VR8J-MbAkrgqu4J#V zma>#2_dnEmXPwKazWDUv7JbdNn>WiWn_k2GTnBN7=&fnbpxpTp^`fbXkBv$(JW1${6eD@A>W557Gu9unwy@}c-3hl<;NG4Q5+wx%qOQL40+Lv(>(Ah@<9%+a zzf$cDfocy^=N-HxC$}<|mema!FV9MSWf3jORCm6Dz9&TTvORB_ETPETk2bSf`6eeP z=eYy4BLdN^@YsGv;*hH)wzx!?j*K#W|LOG4e90B2t>gQA5TaINhpzJ4mM*j-N?g#+ zqef$<{VQ(rs;tC*<|CIu#=3*XW_@FW%wy--Kcug)0}#Y?wYB|@3EKgYdGFq{YZopC zefjd`c#OUxGCuCp(RV`G5j$MONP+_%8b(GZ4UEP_9A?)p^FUdLF{#>3c>@=U!HV|f z!aZU)RK~i7BjP;Sy@0Wf4DD=mrV0uQEBrio60n#V@JtNaXs2m%k6wS{ey)KiGYMF} zTKFCJFoQtI+KbB`07&uPEnqj3zDy=dtKbt52U?BFh&Ne>Cb2Y}MDhZSC;{h+d?Kz} z)qz99VIDRIj(rZehs{1t81p5*WY|mYd6_mufDhU1wI%QcppLBIOPVuW%fg*1;3_J9 z0}Ed7%Ui;q!AsybT?b!-R~}}N4~~xJ_NT#&{UBOl+{pT3I?8qSL?`SLK4i#da3_vK z>n9U=NG!^PJBcrxK1H5Bfy+bidjW*gb?kAa4_=7wzN)%fPD!Z@`v@PyS3epPa1SJs zu&|&(P_umeQ{cmE{LtwBuz-Vqk&<%e&0%id=OfXDh9i&AuQ=WD`7=CTYy)h>PI6Kd z#23+=0EZ3g2V#eICL$f}#6jjn7vUtOXwar};NysfB%Iz|bwx+8Hg%tgI|9Q}jIezr zE|sjPV4e00WW8}^^~K-;Z{uLuLl5QXL+{Gb6T52Zu&}U2sGG#&^D4IpyfD}wWS+L< z0wP$+AhkJ&TaEp*3vd0nFTMfxU-x7wM9uLSRdHr!6L{oY0%|KN%)lb6X=`e>W8=8V zAs{T{Egj~GHy8`5=KwdL*8Y$7&OIvU^zZj~N*V^ClybHJ^Qa`t-ZFjX04gQ z-F@HR>-t=u&--+F51P!8FlL6YLRjR0&atizoo4uI^mlo>;~s&gX@$mz$FHdrS{C*u zuXgH`#=?aQQBhz^WYdzVcz4j-n8S$E$d4POs5q7!;E!X@_nZj)B#i)5We2`vHnG9TR{oFb;UUmg^P(!|rN4sAS7fCK7vvlj( z_$)x7&ve`S9F1*aC~-(4&E#prbdo1tpHl$fR!dXE@)L%ynbB6QK!0wnNA7OWM%{yM zCcQ{1ea*L#@Juo8Q9s<5$LIXusKPAs1n;AvGF)U}|Nr0ozxfibCJtKmFD-yX^56MY z9@?7NN0Pr4x14aA8g@G%Je(KLGQKx7{Pt*sw8i#_2t}Ef0KV`#20o*z9jvXx6Ky5l zb3F7_Y-=P@8(5kmr7kf|B2m7Jm5=$(I=XYdj_fVIlK;bJlO@!vvn`q0x%f=B)Hed2|XVjckS}?3tFeKKG=?3(1h1wl}}7F&1CT@U!7Q=^(fv- z3ARO|8-}cZw4`@}1-}tYH^-iV@L5_nMIs4xUAgiKXdXlRLBn2Aev`f{=qg88rh&v^ ziUCZd8987$Kw|oF0x6&#k3{4Y5K}@y=NN>E5=qqG?&JTL7XE?*hEkH?;~x5lAm4zT zr%svj7q-2!vP&R6V&EpC2mHk>d7yCs4bUxsSH#j6Ijn#!D9|dC)}P%ug0#(3`YI8L z>)8KxE2^Jz)Z~1vOJb$G-h@hQ)wPh$^f6D$+5t#Fp4%F3^vul6+;CVl0Q?@{`K^3q zi)&B2NA**YXq0jaAELU%P}H7mfoC!p>&Y zcV(21oR;7@kBW-ua12P)t2GjO?pMiZC~1Cl1o89)XdRM?8J$2>Yz?H^uW)lymz&HP z21;-3vuM|EzS7I&hEEJ`n{#fj+dr9<5H;4r2w7NZ2piLzLK(2rTLj(DmKoR z-vSqqSmHTgDN*{nD;_{Je&qaRLT24=*k(yQlSlXCB>aqh-7CiQ$TQP^YztFW#f2Nr z!z!7{!=(~PW%X<~)OBRV15Q!w#M9-saNxd*@Lsp>(bt|2Xnw2W{+KL2hQgjnomvLb z@rclN#s#oYd=uVN4B$c>EAT!-Ulw~d(^oall4Z&~o>86gWQm^Lb=?>~?DRb&@GuU> zQUl0-X)_$x2yJ3h`1*KSVJ_;8zDXI?>~ZZisTyMnR>izy(>DZ(kIEQ3g2|Ws;N7=| zzij|*PS(c1baz}T<%%`B$DSxtJ-VgHSKFP%m!y9*u!r#WEa2_BZ|!FXuk}QAT`Evj`-v%4o{`p z_RrBWz*GFL6o~f%Q+@#ED7zJ1)4^E)y|Jg;(PkcNq1DQB*w|b*4^A~;C^HFO&?VAw zzgJfB&J1~c9TvIv1>0R5I!Q}6hnuW>9UhQDza4siYO?(xlfVGFL>}W*YwLSJLD5H! zKo~uw;p0pCOfjP!3Vl|kWj)x<&(bn<>(;I9W3MotR(4PT} z>`{oZaTQDQ(}+IhY!@*E?hhv)YawO<$7`Oj7Uu@H9>Hgdtp*l4OihlS4>#fx%4%o^i9r*k8wpAc=zq7YI(It?@1Zw+HC2 zKXiLQ{vFmdH!j=_z8pAwWX-!#b*^LB8t7+bpN_S8yk_r!U^ThPpq*?0Xs-mfHm2Yed1P38%aA6|69r#Fi!|ghy477-T_SLH>E)U@@;Ja1O zli*HtSMoB;>{ieXzUP@h*CHxewd#zT9Q`pM%m($q(7fpJYbIxVo{IebFCjxY!39ut z*`rzL%rcn3}gSbg6}fg-1lR;63s#&-rmq(hljAAxI?(QcN zqEOmfM;m6)W^a{cp38s<1E}lkfNfrnq<91oktgRA^s9Zhul&CtnFxJ0puv+f+{V@N zbg7Mff0MvPd}*1^6mM<0>-c-H;g;nDbudrAIeKf&)nKW)c}IT;9aIz)Tz6ULr_CLI zxeF`8{ejwz&p%}~Dl1FJ+zGck7yGAf(berVNM`bgq*<$6i7*Db_Sr{plg1KYRpnt3 zYj>up+Kgb|TP!b&J)@h9$N;>|Dhc}|b_m(p1zCH{QqlT9`v_98C*^hYA^F^^Pk9;~ z8plU3$qgN2_EvKR@w?R3<-3joi@Zw0lX!B zd=nEB^X9*>MGQ#VV1#l!JKx2|O1zO@R+)6R(Z7{cYBR$9dl&cT*;iuym*C?D*@5ew zr#X+@Gh#5kZ$H1X(IMNBz+>}eb62hLl@80D_xBIT_e0>uE^F=UYZX-10+{VSsXL<;`dP-3>YIagwlI_WzNA)D0 z`iHQDf|Z`&Xb7EQ`F3zL%81aq%mB^rX%mMGamGvruFnX6h=B<|5ePlp01(xV;+{4G zxhTi9SJYW&3U{KEmO)guQ(&t_c3G@>7b8`n#;wokAa3pKW21!JeFqLGtEgxub4SmW zPX!ZYYcv1Syq`7^=L+-Q!gJ1B9g__p@T@+g>);o@B`S$*mo66&|J}JQ$+~ngzekMR zjQQsn#>#CJRcAa0lgl`LiQ-ZB)R&DCqW1H^Nu@Sp` zrdgjOP+JqB<+5{8YaMH!ea!efJiP(vEB`vY&|ZGay?fRlgQ8CHpUjhgf7Hu#^qepg z0o*AzB$khw*gsO%B*3Ug-{gWF`m+jC9nM_~riY?k05wPjup?Qy z4MGY0?~ZE1L-)qVv%mg)1YYVW`U!9me7Q@<^xcB~=6Oz#(<1zibO)Pb{R5jd8s5#C zF@qpPSFs&9VQh-E zwcC3Wq=8NyU-!?5Hws$HK*S;Z zB6nSM0(%>Ho^hie{gdAht!phgk)C#$5(yS4)Da)f0yuKihIt4*3bp^McaN+xo_LfW z-*`~Ni+=>%r4)Y(rFaWlprhhpZZS&lK(fUas>bId^oT#%ZXJzK!DD6K#0)235_k<6 zGg5~a@Ri2D59;`S;X?tuYBTX*tt~BW-=23EOi2}}B`Dy{IqzseKy2a8^rsc342zF` z2KQMmny_0DKHCWBn|@;jt-yIA+cp_?S+XXfy+CsHX8Ne>KZQI%^pbo4qGXL6EbOO2 z8*&9n`7HVe4Xp-J7?+UHccQs*YIr^_m-aqq&`*N$WjGtRypTqV(ex(vp}X&gzeaWz zO{}4RJE(-6rjYJ*wAKtVUp8@BzQUR(;1VO7oVBBZe3f32#4>jTY~Hx_9i|6w)SJX) z*#_oX=B=_Tl6OVrMz$mCxiyL71mjU}tu%_p8W+OuN~d5m8NX)8>SLTMgw&^~$)PSH z4lN;eu40!Oxey!59SSYw)8!5a8Vf&!xPOe-e+RY}k;ag9Q`Y5M!b5d1H-~$O> z=xlZhS2~R^+2Fg#GupdXQBg5t=?&R6KbzAhqI`-ULFOPZPM_z!*kp_#i_q0*Yrp5T z$BDZh^~}w@cAos_rdnI`>Q)w|9iB}2E&MN$Vq)_wyG(rGDQ{zEF(-^E+8LZ0@;u0N z!^yU=h{$K93>^%ayX#Ar@#?U>`VePHz-|S8&o|XnRz_af-ZE%8dzYanG`|5z2{cks zCYFx4u_Nrk%vLu)GeSUKb)0}rbIaN!wJc=Dr<|M~Qc>pkqz-~Y;V1YMs5alxQ6x4X z%4A11zUS4rK&6VXRIFp4d|`5L#Kgof7Zl#RC;4*muGMrNbq#+Ny~Q_51I-+hl|3F& zf?k+FsBC?Uhe7x0>o>7Ipy7f(zgxh!gvN!K$8!2C)mO<%_?-mRq^T(^dt^!_J^r=l zn}mk6%WCY9#x8nT+urcLZ9lmkHXJ}s&lN`Fq&$W--Q0G*ZDl6=D z=3z1m+!^FXlE#txFI{5VIGAI+quy29<7*^zoS6*7lMwH|-MZ}v8G=ki>`;UyMbX8I}gDd~n3}fK?j3+bH$0_V>>#>FM zRQG5Y_d{hL`m#OCbi;|a_2FO(*_5VP+=d2zNrf({nRT1)0bP_$s@6U;F`FX?NMh6|bN!joFI@0rgS90#ul39@gjVPA5@^6YKqDBUrmJgt4vrcMoO3Cf z{qWT?$LRV#K@FY^|CSIPNBQ8zp$wSpCvI`lwY1lW zjf;!p`HK+JZG!?Is0VUVbCf9V*VfkNdDcPZ+>`monBa5oBnsMt2b*P+2drnitdq;e zww5&_IvnyxjQ(O*WN@huO`C0ko&HQ~ss*Z;bhbEn)m~ZkpV?>YDx{sz1 zNcoWFAmtKBDMqJ7S?9yU=3K-k^BYVW1FFh|wXSr{W)#%_$bl7A`^Fq`;a0rPT!_(x zfD0cs^FLSm;H*3LMGt-8pFkjNG|&kK1mF&4{&CaK>;A&f=xj2Ih+B8&nb7-PLif@u zDk!AGwWCz{ncZ{q_g?7J*%*%DwWGN-)d8J(Og*zBpEq(Q@@OE>R+q9FJ$c9gxa3Qg4)~uZuRZ4?4OrlUpLnUZ|)f_EnkOa0HK^~*nT9A!&H5ZCiw>9>Skod42 zt&-X_Wn!RHBsOvpmM46+Igd7;kRfaR>8Y0^bXB8CWP`87W;{hpV^LnWRWp3!cjpYv z8X!^F+3!bMq{WFJn5P0*ENjF@pFc=k>O+km)}sIMoA<956Cas>p3lG(AQGB5Z9FqB z@ztbU`CLYqt~%D!wXQgP2$!C(}wD<+LjJ!Ntnu>l&d9Us9?N-@#1 z0-c>JMOX5(vYJmbp5&>>7+%Zp0hmy6fZl;?r0zd_IOpwb z`JzHM;1kY%5p~OVoqK4Z{1&J{g{b+I9eSR)SsaM#_=h<>EQRE)_b3hY_4KHA2ON_} z6{-4D46*h9er?dEra2^7*k4655nq9cU`6o_8JixQs-`cSbS`!;H7A3Rx?i1KMwayI z_el@kLBitC{*7_*;dAfU4g&K_xHyzc9Jm1~` zBEfz1t^E8tx(rK0Ye_Xjw?tu9(u=_4H(?Gt#^x0#_ss+JY}23Y zjd)33abzRmWf>n{;C7gHjivUW7hx5PTMN}m#9%QMec6NUf0~X+1YLO)Z{y3CgJroK z@ICj_-OeWBW$@1Oho9c>YLEMM`SOG+t$Yq4_`cjbKxC>_x>8XK#pWG!tOYfFH=|wj19h#wpl3RBOQG@;FE8}i61jD&MRriN z#!FV8-w$n3 z@i4OBr^dC_RMpiTii{i*)C}^<2>C}?Sbli}H?HncvFFCC+e*@T3|Y+q6G>)_*G)F7 z+pwvT8m1=n+ZsLi^de;?uz*v8{tBp?TC~(~|`=SA7wqD{X(Z$A<1gID0Y1O~dZZ#x^ zD*$HP!Y=@7w|cu`Sx)=5pJI+shl5iG!@xpQRW?#$!5DSquCxABKr(yx9&&8O&1tBv zhB%dcnw$X!6@V1{#@BSNuXv=F1Z_MoyNvPCNVTqtCcklBvP+&-yB^V$I_W{*d-gG% z;I_1mRNk*;j1M9){CPH*7@nkoN=iTA;T=kTXfzZ)pJ|S=HPWdoFDuJ;AagQqcGBb& zH>>nf0%%l$l zZOjQ*^i*67@hZtNW4&4W^>o9Lzqe+mi!=> zdPgt+J=n)KIeP3B7kU`D&I(a$mmh|kLH1qN(i@?T^Ei$ZL>srva{c-i-UX^#*0Ljv z9EZd9LI;w1m34nLCGmaE7gnTS>*=E>ifEC4aeDta`S9R%y=9UwPwKuf6+Po!2Y@86 zs|8Y~L1K9uU||QT&LI|pciHzsOKYpRfu*vM=7THWor`5KZD>2;rMYCBOkE!7Bsjvs z>+&Bhg%OsT2+F*_u#OYk!bp|NKDl>NMYoTnS~M_GQXm0nI4-27mT}`j!0COWy>$o? zS}pL5s_GY5BWh~YliM-hd`*umJj`uf<%AyHSdJ%G>sh$gtAmrWw;&sAEtpt8EeRcB zaoS680pUKnYAuOc_hWyOkM)uB7_b?zX8&p;2SNAIohmxqhTy8Y)x9cyG<(`(0;=8z z8;uA#qYb$ba9A}gGgGu+@g)qpwU1_WT(-+;sq9ue`5UjpfwjwTMD^QGuTBPhIb?M~xAuS+B{i>6O8vm2QVvgR%f76G&z;Ep+ z%VFFuI*pf}G-;ACCrQ9%%^xSCUBG-v@IE*Lz`kX!uK3d`IoVnZRNYCLh9G6~=3+&d z@|B2S+dY9TI5=4iki=rdthe2US(4Y}JQC9(6dAkW_i`Gr7JN!v52Chu4=m%m@(Z)Q zs_2Tasq|A+jL%riO-DH6AonIrh@P;{vaAB<0{q_fpspRqfC?&Bi<=dMDRj_F*JaDF zvz!%}(A~RMCmtfr=069IA3t7Qed-V5l{*K_-;O>m8c|euWZ9SmwCL6={fzAqvl$>B z{3s?MvtxFo%u|97b9<%SbpVKzFc`0UqQXyXM+JjUITg zNyS8>@sOVv)@0^Lv}W?aIs{X31BLT8qokME5?N0@7hk-;e^NzWUgES&I8LRe&1HDG zb`DJ@>6`-V-FAcs*rw^f;y3cYvQmY4hdX)#M;kSTcuAg4#h2xJXR~=OEY< zC&h^2!xfv>*(cgjU>-c|SdGZA%qyk3{BcpyTG|{ikoeyYNb*(BaFVp9a&&^Yf3L#) z<2^fYEvu4Ndn&y)cbeLfQ0K{`&{b25VR;Xqb^ZX%#IPlwn1_}k(cZD+N4mJ*he^&i z<@oMLi%hkQ#@~E=_7=u2*oEvOG&Gc#LVQtLRFukq#l)Zt-t$;~gG5*1@9FH~cZL7` eu=c(~YE8+^#?zbI^7uoN=@zr5oSFP<$iD%NC@?Vq literal 0 HcmV?d00001 diff --git a/docs/update-delay.dox b/docs/update-delay.dox index f978f47ae..478352b3f 100644 --- a/docs/update-delay.dox +++ b/docs/update-delay.dox @@ -4,6 +4,9 @@ @page update-delay Delayed Feature Initialization We describe a method of delayed initialization of a 3D point feature as in [Visual-Inertial Odometry on Resource-Constrained Systems](https://escholarship.org/content/qt4nn0j264/qt4nn0j264.pdf) @cite Li2014THESIS. +It is important to note that this is just an example of how to leverage delayed initialization which is very pertinent to visual-inertial state estimation. +In practice, as long as sufficient measurements are available to constrain the new state (e.g. ENU-to-VIO transform, sensor calibration, object pose, etc..), the below delayed initialization procedure can be applied directly. + Specifically, given a set of measurements involving the state \f$\mathbf{x}\f$ and a new feature \f$\mathbf{f}\f$, we want to optimally and efficiently initialize the feature. \f{align*}{ @@ -85,6 +88,9 @@ For example when initializing a new feature to the end of the state, the augment Note that this process does not update the estimate for \f$\mathbf{x}\f$. However, after initialization, we can then use the second system, \f$\mathbf{r}_2\f$, \f$\mathbf{H}_{x2}\f$, and \f$\mathbf{n}_2\f$ to update our new state through a standard EKF update (see @ref linear-meas section). +We note that if there are exactly enough measurements to initialize the state, then no update occurs. +This make sense, as the system is not yet overconstrained, thus only the to-be-initialized state and its uncertainty can be found (it does not yet benefit the navigation state estimates). +Some additional notes on performing delayed initialization can be found in [this](https://pgeneva.com/downloads/notes/2022_notes_delay_init.pdf) technical report. */ \ No newline at end of file diff --git a/ov_core/src/cam/CamBase.h b/ov_core/src/cam/CamBase.h index 60788f903..da1cbcf73 100644 --- a/ov_core/src/cam/CamBase.h +++ b/ov_core/src/cam/CamBase.h @@ -34,10 +34,7 @@ namespace ov_core { * * This is the base class for all our camera models. * All these models are pinhole cameras, thus just have standard reprojection logic. - * - * See each base class for detailed examples on each model: - * - @ref ov_core::CamEqui - * - @ref ov_core::CamRadtan + * See each derived class for detailed examples of each model. */ class CamBase { diff --git a/ov_core/src/utils/opencv_lambda_body.h b/ov_core/src/utils/opencv_lambda_body.h index a4c85bb5e..1c4cbff8e 100644 --- a/ov_core/src/utils/opencv_lambda_body.h +++ b/ov_core/src/utils/opencv_lambda_body.h @@ -27,7 +27,7 @@ namespace ov_core { -/** +/* * @brief Helper class to do OpenCV parallelization * * This is a utility class required to build with older version of opencv diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index fd2d3921f..568f7011b 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -43,13 +43,16 @@ namespace ov_core { * @brief Helper class to do OpenCV yaml parsing from both file and ROS. * * The logic is as follows: - * - Given a path to the main config file we will load it into our cv::FileStorage object. + * - Given a path to the main config file we will load it into our [cv::FileStorage](https://docs.opencv.org/4.x/da/d56/classcv_1_1FileStorage.html) object. * - From there the user can request for different parameters of different types from the config. * - If we have ROS, then we will also check to see if the user has overridden any config files via ROS. * - The ROS parameters always take priority over the ones in our config. * * NOTE: There are no "nested" yaml parameters. They are all under the "root" of the yaml file!!! - * NOTE: The camera and imu have nested, but those are handled externally.... + * This limits things quite a bit, but simplified the below implementation. + * + * NOTE: The camera and imu have nested, but those are handled externally. + * They first read the "imu0" or "cam1" level, after-which all values are at the same level. */ class YamlParser { public: diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index 83ce3c211..3eae82bcc 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -26,7 +26,8 @@ * @section Summary * This file contains the common utility functions for operating on JPL quaternions. * We need to take special care to handle edge cases when converting to and from other rotation formats. - * All equations are based on the following tech report: + * All equations are based on the following tech report @cite Trawny2005TR : + * * > Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation." * > University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005. * > http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf @@ -48,13 +49,13 @@ * ~,~ * -\mathbf{k}\mathbf{i}=\mathbf{i}\mathbf{k}=\mathbf{j} * @f] - * As noted in [Trawny2005] this does not correspond to the Hamilton notation, and follows the "JPL Proposed Standard Conventions". + * As noted in @cite Trawny2005TR this does not correspond to the Hamilton notation, and follows the "JPL Proposed Standard Conventions". * The q_4 quantity is the "scalar" portion of the quaternion, while q_1,q_2,q_3 are part of the "vector" portion. * We split the 4x1 vector into the following convention: * @f[ * \bar{q} = \begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix} = \begin{bmatrix}\mathbf{q}\\q_4\end{bmatrix} * @f] - * It is also important to note that the quaternion is constrainted to the unit circle: + * It is also important to note that the quaternion is constrained to the unit circle: * @f[ * |\bar{q}| = \sqrt{\bar{q}^\top\bar{q}} = \sqrt{|\mathbf{q}|^2+q_4^2} = 1 * @f] From 5b1194c383a0e16ec5019f149c2cf80a57e3254b Mon Sep 17 00:00:00 2001 From: yangyulin Date: Wed, 10 May 2023 11:12:53 -0400 Subject: [PATCH 43/60] small fixes to propagation docs --- docs/propagation-analytical.dox | 69 ++++++++++++++++++++++------ docs/propagation-discrete.dox | 80 +++++++++++++++++++-------------- docs/propagation.dox | 10 ++--- 3 files changed, 107 insertions(+), 52 deletions(-) diff --git a/docs/propagation-analytical.dox b/docs/propagation-analytical.dox index f59a40487..032f0aa04 100644 --- a/docs/propagation-analytical.dox +++ b/docs/propagation-analytical.dox @@ -14,6 +14,7 @@ Key references for analytical inertial integration include: - High-precision, consistent EKF-based visual-inertial odometry @cite Li2013IJRR @cite Li2012TR - Closed-form preintegration methods for graph-based visual-inertial navigation @cite Eckenhoff2019IJRR @cite Eckenhoff2018TR - Analytic Combined IMU Integration (ACI^2) for Visual-Inertial Navigation @cite Yang2020ICRA @cite Yang2019TR_ACI +- Online Self-Calibration for Visual-Inertial Navigation: Models, Analysis and Degeneracy @cite Yang2023TRO @@ -44,24 +45,33 @@ to prevent unneeded matrix inversions and transposes in the measurement equation We only calibrate either \f${}^I_w\mathbf{R}\f$ or \f${}^I_a\mathbf{R}\f$ since the base "inertial" frame must coincide with one frame arbitrarily. If both \f${}^I_w\mathbf{R}\f$ and \f${}^I_a\mathbf{R}\f$ are calibrated, -it will make the rotation between the IMU and camera unobservable due to over parameterization @cite Yang2020RSS. +it will make the rotation between the IMU and camera unobservable due to over parameterization @cite Yang2020RSS @cite Yang2023TRO. We define two different models of interested: - *KALIBR*: Contains \f$\mathbf{D}'_{w6}\f$, \f$\mathbf{D}'_{a6}\f$, \f${}^I_w\mathbf{R}\f$ and gravity sensitivity \f$\mathbf{T}_{g9}\f$. This model follows IMU intrinsic calibration presented in @cite Rehder2017Sensors and the - output used in the open-source calibration toolbox Kalibr @cite Furgale2013IROS. + output used in the open-source calibration toolbox [Kalibr](https://github.com/ethz-asl/kalibr) @cite Furgale2013IROS. - *RPNG*: Contains \f$\mathbf{D}_{w6}\f$, \f$\mathbf{D}_{a6}\f$, the rotation \f${}^I_a\mathbf{R}\f$, and gravity sensitivity \f$\mathbf{T}_{g9}\f$. - \f$\mathbf{D}_{a6}\f$ and \f$\mathbf{D}_{w6}\f$ are uptriangular matrices, and follows *imu2* analysed in @cite Yang2020RSS. + \f$\mathbf{D}_{a6}\f$ and \f$\mathbf{D}_{w6}\f$ are uptriangular matrices, and follows *imu2* analyzed in @cite Yang2020RSS @cite Yang2023TRO. -We can define \f$\mathbf{D}_{*6} = {\mathbf{D}'_{*6}}^\top\f$ and the following matrices: +It is important to note that one should use the *KALIBR* model if there is a non-negligible transformation between +the gyroscope and accelerometer (it is negligible for most MEMS IMUs, see @cite Li2014ICRA @cite Schneider2019Sensor) +since it assumes the accelerometer frame to be the inertial frame +and rigid bodies share the same angular velocity at all points. +We can define the following matrices: \f{align*}{ \mathbf{D}_{*6} &= \begin{bmatrix} d_{*1} & d_{*2} & d_{*4} \\ 0 & d_{*3} & d_{*5} \\ 0 & 0 & d_{*6} - \end{bmatrix} \\ + \end{bmatrix},~~ +\mathbf{D}'_{*6} &= \begin{bmatrix} + d_{*1} & 0 & 0 \\ + d_{*2} & d_{*4} & 0 \\ + d_{*3} & d_{*5} & d_{*6} + \end{bmatrix},~~ \mathbf{T}_{g9} &= \begin{bmatrix} t_{g1} & t_{g4} & t_{g7} \\ t_{g2} & t_{g5} & t_{g8} \\ @@ -226,6 +236,30 @@ The mean propagation for the new state at \f$t_{k+1}\f$ can be written after tak @section analytical_linearization Model Linearization Derivations + +@subsection analytical_linearization_error_stats Error States Definitions + +We first remind the reader of the error states used throughout the system. +It is crucial that all error states are consistent between propagation and update. +OpenVINS has a type system (see @ref ov_type and @ref dev-index-types) for which Jacobians need to be derived from. +We have the following pose (orientation + position) errors (based on @ref ov_type::PoseJPL) + + +\f{align*}{ +{}^{I}_G \mathbf{R} &\simeq \exp(-\delta \boldsymbol{\theta}) {}^{I}_G \hat{\mathbf{R}} \\ +&\simeq (\mathbf{I} - \lfloor \delta \boldsymbol{\theta} \rfloor) {}^{I}_G \hat{\mathbf{R}} \\ +{}^{G}\mathbf{p}_I &= {}^{G}\hat{\mathbf{p}}_I + {}^{G}\tilde{\mathbf{p}}_I +\f} + +For all other vectors (such as IMU intrinsics etc), we leverage simple additive error (based on @ref ov_type::Vec) + +\f{align*}{ +\mathbf{v} &= \hat{\mathbf{v}} +\tilde{\mathbf{v}} +\f} + + + + @subsection analytical_linearization_imu IMU Reading Linearization We first define the \f${}^{I_k}\tilde{\boldsymbol{\omega}}\f$ and \f${}^{I_k}\tilde{\mathbf{a}}\f$ error states. For \f${}^I\hat{\mathbf{a}}\f$ and \f${}^I\tilde{\mathbf{a}}\f$, we have: @@ -317,7 +351,7 @@ For \f${}^I\hat{\boldsymbol{\omega}}\f$ and \f${}^I\tilde{\boldsymbol{\omega}}\f \f} where we need to define \f$\mathbf{H}_{Dw}\f$, \f$\mathbf{H}_{Da}\f$ and \f$\mathbf{H}_{Tg}\f$. -For the KALIBR model, we have: +For the *KALIBR* model, we have: \f{align*}{ \mathbf{H}_{Dw} & = @@ -335,7 +369,7 @@ For the KALIBR model, we have: \end{bmatrix} \f} -For the RPNG model, we have: +For the *RPNG* model, we have: \f{align*}{ \mathbf{H}_{Dw} & = @@ -580,8 +614,8 @@ For the integration of \f$\Delta \mathbf{v}_k\f$, we get: where \f$\delta \tau = t_\tau - t_k\f$ and \f$\boldsymbol{\Xi}_i, i=1\ldots4\f$ are shown in the @ref analytical_integration_components section. -\f$\mathbf {J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\f$ is the right Jacobian of \f${SO}(3)\f$ -[see @ref ov_core::Jr_so3() and @cite Barfoot2017]. +\f$\mathbf {J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k)\f$ is the right Jacobian of \f${SO}(3)\f$ +[see @ref ov_core::Jr_so3() @cite Barfoot2017]. In summary, we have the following linearized integration components (see @ref analytical_integration_components): \f{align*}{ @@ -697,9 +731,12 @@ where \f$\boldsymbol{\Phi}_{I(k+1,k)}\f$ and \f$\mathbf{G}_{Ik}\f$ are the state \end{bmatrix} \f} -When using FEJ (see @ref fej for an overview) for the state transition matrix, -we need to replace \f$\Delta \hat{\mathbf{R}}_k\f$, \f$\Delta \hat{\mathbf{p}}_k\f$ and \f$\Delta \hat{\mathbf{v}}_k\f$ as: +@m_class{m-note m-frame} + +@par How to apply FEJ to ensure consistency? +When using First-Estimate Jacobians (see @ref fej for an overview) for the state transition matrix, +we need to replace \f$\Delta \hat{\mathbf{R}}_k\f$, \f$\Delta \hat{\mathbf{p}}_k\f$ and \f$\Delta \hat{\mathbf{v}}_k\f$ as: \f{align*}{ \Delta \hat{\mathbf{R}}_k & = {}^{I_{k+1}}_{G}\hat{\mathbf{R}} @@ -722,8 +759,13 @@ we need to replace \f$\Delta \hat{\mathbf{R}}_k\f$, \f$\Delta \hat{\mathbf{p}}_k + {}^G\mathbf{g}\Delta t_k ) \f} +This ensure the semi-group property of the state transition matrix by ensuring that the previous timestep is +evaluated at the same linearization point (their estimate before update / their estimate when they were first initialized into the state). + + -Note that \f$\mathbf{n}_{d*}\sim \mathcal{N}(\mathbf{0}, \frac{\sigma^2_{*}\mathbf{I}_3}{\Delta t})\f$ and hence the covariance for \f$\mathbf{n}_{dI}\f$ can be written as: + +Note that \f$\mathbf{n}_{d*}\sim \mathcal{N}(\mathbf{0}, \frac{\sigma^2_{*}\mathbf{I}_3}{\Delta t_k})\f$ and hence the covariance for \f$\mathbf{n}_{dI}\f$ can be written as: \f{align*}{ \mathbf{Q}_{dI} & = @@ -927,7 +969,8 @@ identities as (see [L'Hôpital's rule](https://en.wikipedia.org/wiki/L%27H%C3%B4 \boldsymbol{\Xi}_3 \f} - +We currently do not implement a [RK4](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods) version of the above integrations. +This would likely improve performance if leveraged as it would allow for the measurement to evolve over the integration period. */ \ No newline at end of file diff --git a/docs/propagation-discrete.dox b/docs/propagation-discrete.dox index 56061d43f..cec709818 100644 --- a/docs/propagation-discrete.dox +++ b/docs/propagation-discrete.dox @@ -156,21 +156,21 @@ That is, \f} For the orientation error propagation, we start with the \f${SO}(3)\f$ perturbation using \f${}^{I}_G \mathbf{R} \approx (\mathbf{I}_3 - \lfloor ^{I}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I}_{G} \hat{\mathbf{R}}\f$ -(which can be shown to be exactly equivalent to the state's quaternion error @cite Trawny2005TR): +(which can be shown to be exactly equivalent to the state's quaternion error, see @cite Trawny2005TR and @ref ov_type::JPLQuat): \f{align*}{ {}^{I_{k+1}}_G \mathbf{R} &= \text{}^{I_{k+1}}_{I_{k}} \mathbf{R} \text{}^{I_{k}}_G \mathbf{R} \\ (\mathbf{I}_3 - \lfloor ^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k+1}}_{G} \hat{\mathbf{R}} -&\approx \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) +&\approx \exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k - {}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t_k) (\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} \hat{\mathbf{R}}\\ -&=\exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t)\exp(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t) +&=\exp(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k)\exp(-\mathbf{J}_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k){}^{I_{k}}\tilde{\boldsymbol{\omega}}\Delta t_k) (\mathbf{I}_3 - \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor)^{I_{k}}_{G} \hat{\mathbf{R}}\\ &=\text{}^{I_{k+1}}_{I_{k}} \hat{\mathbf{R}} - (\mathbf{I}_3 - \lfloor \mathbf J_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t) - \tilde{\boldsymbol{\omega}}_k\Delta t \times\rfloor) + (\mathbf{I}_3 - \lfloor \mathbf J_r(-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k) + \tilde{\boldsymbol{\omega}}_k\Delta t_k \times\rfloor) (\mathbf{I}_3 - \lfloor ^{I_k}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) \text{}^{I_{k}}_G \hat{\mathbf{R}} \f} @@ -185,36 +185,40 @@ By neglecting the second order terms from above, we obtain the following orienta \f{align*} \text{}^{I_{k+1}}_{G}\tilde{\boldsymbol{\theta}} \approx \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} \text{}^{I_k}_{G}\tilde{\boldsymbol{\theta}} -- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t (\tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{{g},k}) +- \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r({}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) +\Delta t_k (\tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{{g},k}) \f} +where we have defined \f${}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}=-{}^{I_{k}}\hat{\boldsymbol{\omega}}\Delta t_k\f$. +This describes how tht error of the next orientation is related to the previous, gyroscope bias, and noise. Now we can do error propagation of position and velocity using the same scheme: \f{align*}{ ^G\mathbf{p}_{I_{k+1}} - &= \text{}^G\mathbf{p}_{I_k} + \text{}^G\mathbf{v}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 - + \frac{1}{2}\text{}^{I_k}_G\mathbf{R}^\top \mathbf{a}_{k}\Delta t^2\\ + &= \text{}^G\mathbf{p}_{I_k} + \text{}^G\mathbf{v}_{I_k} \Delta t_k + - \frac{1}{2}{}^G\mathbf{g}\Delta t_k^2 + + \frac{1}{2}\text{}^{I_k}_G\mathbf{R}^\top \mathbf{a}_{k}\Delta t_k^2\\ ^G\hat{\mathbf{p}}_{I_{k+1}} + \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &\approx \text{}^G\hat{\mathbf{p}}_{I_k} + \text{}^G\tilde{\mathbf{p}}_{I_k} - + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t - + \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t - - \frac{1}{2}{}^G\mathbf{g}\Delta t^2\\ + + \text{}^G\hat{\mathbf{v}}_{I_k} \Delta t_k + + \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t_k + - \frac{1}{2}{}^G\mathbf{g}\Delta t_k^2\\ &\hspace{4cm} + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top (\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) - (\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t^2\\ -\\ -^G\mathbf{v}_{k+1} &= \text{}^G\mathbf{v}_{I_k} - {}^G\mathbf{g}\Delta t -+\text{}^{I_k}_G\mathbf{R}^\top\mathbf{a}_{k}\Delta t\\ + (\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t_k^2\\ +\f} + +\f{align*}{ +^G\mathbf{v}_{k+1} &= \text{}^G\mathbf{v}_{I_k} - {}^G\mathbf{g}\Delta t_k ++\text{}^{I_k}_G\mathbf{R}^\top\mathbf{a}_{k}\Delta t_k\\ ^G\hat{\mathbf{v}}_{k+1} + ^G\tilde{\mathbf{v}}_{k+1} &\approx {}^G\hat{\mathbf{v}}_{I_k} + {}^G\tilde{\mathbf{v}}_{I_k} -- {}^G\mathbf{g}\Delta t +- {}^G\mathbf{g}\Delta t_k + \text{}^{I_k}_G\hat{\mathbf{R}}^\top (\mathbf{I}_3 + \lfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}}\times\rfloor) -(\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t +(\hat{\mathbf{a}}_{k} + \tilde{\mathbf{a}}_{k})\Delta t_k \f} where \f$\tilde{\mathbf{a}} = \mathbf{a} - \hat{\mathbf{a}} @@ -224,18 +228,18 @@ By neglecting the second order error terms, we obtain the following position and \f{align*}{ \text{}^G\tilde{\mathbf{p}}_{I_{k+1}} &= \text{}^G\tilde{\mathbf{p}}_{I_k} -+ \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t ++ \text{}^G\tilde{\mathbf{v}}_{I_k} \Delta t_k - \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top -\lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor +\lfloor \hat{\mathbf{a}}_{k} \Delta t_k^2 \times\rfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} -- \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 +- \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t_k^2 (\tilde{\mathbf{b}}_{{a},k} + \mathbf{n}_{{a},k})\\ ^G\tilde{\mathbf{v}}_{k+1} &= \text{}^G\tilde{\mathbf{v}}_{I_k} - \text{}^{I_k}_G\hat{\mathbf{R}}^\top - \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor + \lfloor \hat{\mathbf{a}}_{k} \Delta t_k \times\rfloor ^{I_{k}}_{G}\tilde{\boldsymbol{\theta}} - - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t + - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t_k (\tilde{\mathbf{b}}_{{a},k} + \mathbf{n}_{{a},k}) \f} @@ -247,8 +251,9 @@ The propagation of the two random-walk biases are as follows: \hat{\mathbf{b}}_{{g},k} + \tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{wg} \\ \tilde{\mathbf{b}}_{{g},k+1} &= - \tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{wg} \\[1em] - + \tilde{\mathbf{b}}_{{g},k} + \mathbf{n}_{wg} +\f} +\f{align*}{ \mathbf{b}_{\mathbf{a},k+1} &= \mathbf{b}_{\mathbf{a},k} + \mathbf{n}_{wa} \\ \hat{\mathbf{b}}_{{a},k+1} + \tilde{\mathbf{b}}_{{a},k+1} &= \hat{\mathbf{b}}_{{a},k} + \tilde{\mathbf{b}}_{{a},k} @@ -265,13 +270,13 @@ By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_ \begin{bmatrix} \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}} & \mathbf{0}_3 & \mathbf{0}_3 & - \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t & \mathbf{0}_3 \\ +\Delta t_k & \mathbf{0}_3 \\ -- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t^2 \times\rfloor -& \mathbf{I}_3 & \Delta t \mathbf{I}_3 & \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 \\ +- \frac{1}{2}\text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t_k^2 \times\rfloor +& \mathbf{I}_3 & \Delta t_k \mathbf{I}_3 & \mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t_k^2 \\ -- \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t \times\rfloor -& \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t \\ +- \text{}^{I_k}_G\hat{\mathbf{R}}^\top \lfloor \hat{\mathbf{a}}_{k} \Delta t_k \times\rfloor +& \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t_k \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 @@ -284,13 +289,13 @@ By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_ \mathbf{G}_{k} &= \begin{bmatrix} - \text{}^{I_{k+1}}_{I_{k}}\hat{\mathbf{R}}\mathbf J_r(\text{}^{I_{k+1}}_{I_{k}}\hat{\boldsymbol{\theta}}) -\Delta t & \mathbf{0}_3 +\Delta t_k & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ -\mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t^2 +\mathbf{0}_3 & - \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top \Delta t_k^2 & \mathbf{0}_3 & \mathbf{0}_3 \\ -\mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t +\mathbf{0}_3 & - \text{}^{I_k}_G\hat{\mathbf{R}}^\top \Delta t_k & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 @@ -302,6 +307,13 @@ By collecting all the perturbation results, we can build \f$\boldsymbol{\Phi}(t_ \f} +@m_class{m-note m-frame} + +@par How to apply FEJ to ensure consistency? +When using First-Estimate Jacobians (see @ref fej for an overview) for the state transition matrix, +we need to ensure that we evaluate all state estimates of prior seen states at the same linearization point +they were previously evaluated at (their estimate before update / their estimate when they were first initialized into the state). +This ensure the semi-group property of the state transition matrix. Now, with the computed \f$\boldsymbol{\Phi}(t_{k+1},t_k)\f$ and \f$\mathbf{G}_{k}\f$ matrices, diff --git a/docs/propagation.dox b/docs/propagation.dox index e83b8914b..66b3084d1 100644 --- a/docs/propagation.dox +++ b/docs/propagation.dox @@ -44,8 +44,8 @@ the gyroscope and acceleration sensor frames. \f$\mathbf{T}_g\f$ denotes the gravity sensitivity, which represents the effects of gravity to the gyroscope readings due to its inherent inertia. Note that this does not take into account the translation between the -gyroscope and accelerometer frames, since it is negligible for most IMUs (see @cite Li2014ICRA @cite Schneider2019Sensor). - +gyroscope and accelerometer frames, since it is negligible for most MEMS IMUs (see @cite Li2014ICRA @cite Schneider2019Sensor). +The full background and summary of different models can be found in @cite Yang2020RSS @cite Yang2023TRO. @section ins_state Inertial State Vector @@ -80,7 +80,7 @@ while we use the quaternion error state \f$\delta \bar{q}\f$ with a left quatern \cos(\frac{1}{2}\tilde{\theta}) \end{bmatrix} \simeq \begin{bmatrix} - \frac{1}{2}\delta\tilde{\boldsymbol{\theta}}\\ 1 + \frac{1}{2}\tilde{\boldsymbol{\theta}}\\ 1 \end{bmatrix} \f} @@ -92,7 +92,7 @@ The total IMU error state thus is defined as the following 15x1 (not 16x1) vecto \f{align*}{ \tilde{\mathbf{x}}_I(t) = \begin{bmatrix} -^I_G\delta\tilde{\boldsymbol{\theta}}(t) \\ +^I_G\tilde{\boldsymbol{\theta}}(t) \\ ^G\tilde{\mathbf{p}}_I(t) \\ ^G\tilde{\mathbf{v}}_I(t)\\ \tilde{\mathbf{b}}_{{g}}(t) \\ @@ -140,7 +140,7 @@ we can define the solutions to the above IMU kinematics differential equation. \f{align*}{ {}^{I_{k+1}}_{G}\mathbf{R} & = \exp \left(-\int^{t_{k+1}}_{t_k} {}^{I}\boldsymbol{\omega}(t_\tau) ~ d \tau \right) {}^{I_k}_{G}\mathbf{R} \\ - &\triangleq \Delta \mathbf{R}_k ~{}^{I_k}_{G}\mathbf{R} \\ + &\triangleq \Delta \mathbf{R}_k ~{}^{I_k}_{G}\mathbf{R} = {}^{I_{k+1}}_{I_k}\mathbf{R} ~{}^{I_k}_{G}\mathbf{R} \\ {}^G\mathbf{p}_{I_{k+1}} & = {}^G\mathbf{p}_{I_k} + {}^G\mathbf{v}_{I_k}\Delta t_k + {}^{I_k}_{G}\mathbf{R}^\top From e0ba712dde8aadc6eed9774e202723d171ef5f48 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 10 May 2023 11:13:43 -0400 Subject: [PATCH 44/60] update quat docs, expand some details about manifold types in ov_type summary --- docs/bib/extra.bib | 35 ++++++++++++++++------ ov_core/src/dummy.cpp | 5 ++++ ov_core/src/types/JPLQuat.h | 59 +++++++++++++++++++++++++++++++++++-- ov_core/src/types/Vec.h | 5 ++++ 4 files changed, 92 insertions(+), 12 deletions(-) diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index 2f0999276..562b1c25d 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -1,3 +1,10 @@ +@book{Barfoot2017, + title = {State estimation for robotics}, + author = {Barfoot, Timothy D}, + year = 2017, + publisher = {Cambridge University Press}, + url = {http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf} +} @article{Burri2016IJRR, title = {The EuRoC micro aerial vehicle datasets}, author = {Burri, Michael and Nikolic, Janosch and Gohl, Pascal and Schneider, Thomas and Rehder, Joern and Omari, Sammy and Achtelik, Markus W and Siegwart, Roland}, @@ -60,6 +67,7 @@ @inproceedings{Dong2012IROS year = 2012, booktitle = {2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, pages = {1064--1071}, + url = {http://tdongsi.github.io/download/pubs/DongSi2012IROS.pdf}, organization = {IEEE} } @techreport{Eckenhoff2018TR, @@ -78,7 +86,7 @@ @article{Eckenhoff2019IJRR volume = 38, number = 5, doi = {10.1177/0278364919835021}, - url = {https://doi.org/10.1177/0278364919835021} + url = {https://pgeneva.com/downloads/preprints/Eckenhoff2019IJRR.pdf} } @inproceedings{Furgale2013IROS, title = {Unified temporal and spatial calibration for multi-sensor systems}, @@ -86,6 +94,7 @@ @inproceedings{Furgale2013IROS year = 2013, booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems}, pages = {1280--1286}, + url = {https://furgalep.github.io/bib/furgale_iros13.pdf}, organization = {IEEE} } @techreport{Geneva2020TRVICON2GT, @@ -175,7 +184,8 @@ @article{Li2013IJRR publisher = {SAGE Publications Sage UK: London, England}, volume = 32, number = 6, - pages = {690--711} + pages = {690--711}, + url = {https://intra.ece.ucr.edu/~mourikis/papers/Li2013IJRR.pdf} } @inproceedings{Li2014ICRA, title = {High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation}, @@ -183,8 +193,7 @@ @inproceedings{Li2014ICRA year = 2014, month = {May}, booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, - pages = {409--416}, - url = {https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.696.3790&rep=rep1&type=pdf} + pages = {409--416} } @phdthesis{Li2014THESIS, title = {Visual-inertial odometry on resource-constrained systems}, @@ -207,7 +216,7 @@ @inproceedings{Mourikis2007ICRA year = 2007, booktitle = {Proceedings 2007 IEEE International Conference on Robotics and Automation}, pages = {3565--3572}, - url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf}, + url = {https://www-users.cse.umn.edu/~stergios/papers/ICRA07-MSCKF.pdf}, organization = {IEEE} } @article{Mueggler2018TRO, @@ -229,7 +238,7 @@ @article{Patron2015IJCV pages = {208--219} } @article{Qin2018TRO, - title = {Vins-mono: A robust and versatile monocular visual-inertial state estimator}, + title = {{VINS-Mono}: A robust and versatile monocular visual-inertial state estimator}, author = {Qin, Tong and Li, Peiliang and Shen, Shaojie}, year = 2018, journal = {IEEE Transactions on Robotics}, @@ -268,7 +277,7 @@ @article{Schneider2019Sensor volume = 19, number = 10, pages = {3846--3860}, - url = {https://arxiv.org/abs/1901.07242} + url = {https://arxiv.org/pdf/1901.07242.pdf} } @inproceedings{Schubert2018IROS, title = {The TUM VI benchmark for evaluating visual-inertial odometry}, @@ -298,7 +307,7 @@ @inproceedings{Wagstaff2017IPIN organization = {IEEE} } @inproceedings{Wu2017ICRA, - title = {Vins on wheels}, + title = {{VINS} on wheels}, author = {Wu, Kejian J and Guo, Chao X and Georgiou, Georgios and Roumeliotis, Stergios I}, year = 2017, booktitle = {2017 IEEE International Conference on Robotics and Automation (ICRA)}, @@ -318,7 +327,8 @@ @inproceedings{Yang2020ICRA author = {Yulin Yang and B. P. W. Babu and Chuchu Chen and Guoquan Huang and Liu Ren}, year = 2020, booktitle = {Proc. of the IEEE International Conference on Robotics and Automation}, - address = {Paris, France} + address = {Paris, France}, + url = {https://yangyulin.net/papers/2020_icra_aci.pdf} } @article{Yang2020RSS, title = {Online imu intrinsic calibration: Is it necessary?}, @@ -327,6 +337,13 @@ @article{Yang2020RSS journal = {Proc. of Robotics: Science and Systems (RSS), Corvallis, Or}, url = {http://www.roboticsproceedings.org/rss16/p026.pdf} } +@article{Yang2023TRO, + title = {Online Self-Calibration for Visual-Inertial Navigation: Models, Analysis and Degeneracy}, + author = {Yulin Yang, Patrick Geneva, Xingxing Zuo, Guoquan Huang}, + year = 2023, + journal = {IEEE Transactions on Robotics}, + url = {https://pgeneva.com/downloads/preprints/Yang2023TRO.pdf} +} @inproceedings{Zhang2018IROS, title = {A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry}, author = {Zhang, Zichao and Scaramuzza, Davide}, diff --git a/ov_core/src/dummy.cpp b/ov_core/src/dummy.cpp index 52d76b678..a8d379636 100644 --- a/ov_core/src/dummy.cpp +++ b/ov_core/src/dummy.cpp @@ -68,6 +68,11 @@ namespace ov_core {} * }; * @endcode * + * When deriving Jacobians, it is important to ensure that the error state used matches the type. + * Each type updates an update function, and thus directly defines the error state it has. + * A type with non-trivial error states is the @ref ov_type::JPLQuat which has equivalent quaterion and SO(3) error. + * For rotations and on-manifold representations, [State Estimation for Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) + * by Timothy D. Barfoot @cite Barfoot2017 covers a nice range of examples. * */ namespace ov_type {} diff --git a/ov_core/src/types/JPLQuat.h b/ov_core/src/types/JPLQuat.h index 43e546e63..48a547b1b 100644 --- a/ov_core/src/types/JPLQuat.h +++ b/ov_core/src/types/JPLQuat.h @@ -35,6 +35,59 @@ namespace ov_type { * We recommend that people new quaternions check out the following resources: * - http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf * - ftp://naif.jpl.nasa.gov/pub/naif/misc/Quaternion_White_Paper/Quaternions_White_Paper.pdf + * + * + * We need to take special care to handle edge cases when converting to and from other rotation formats. + * All equations are based on the following tech report @cite Trawny2005TR : + * + * > Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation." + * > University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005. + * > http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf + * + * @section jplquat_define JPL Quaternion Definition + * + * We define the quaternion as the following linear combination: + * @f[ + * \bar{q} = q_4+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k} + * @f] + * Where i,j,k are defined as the following: + * @f[ + * \mathbf{i}^2=-1~,~\mathbf{j}^2=-1~,~\mathbf{k}^2=-1 + * @f] + * @f[ + * -\mathbf{i}\mathbf{j}=\mathbf{j}\mathbf{i}=\mathbf{k} + * ~,~ + * -\mathbf{j}\mathbf{k}=\mathbf{k}\mathbf{j}=\mathbf{i} + * ~,~ + * -\mathbf{k}\mathbf{i}=\mathbf{i}\mathbf{k}=\mathbf{j} + * @f] + * As noted in @cite Trawny2005TR this does not correspond to the Hamilton notation, and follows the "JPL Proposed Standard Conventions". + * The q_4 quantity is the "scalar" portion of the quaternion, while q_1, q_2, q_3 are part of the "vector" portion. + * We split the 4x1 vector into the following convention: + * @f[ + * \bar{q} = \begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix} = \begin{bmatrix}\mathbf{q}\\q_4\end{bmatrix} + * @f] + * It is also important to note that the quaternion is constrained to the unit circle: + * @f[ + * |\bar{q}| = \sqrt{\bar{q}^\top\bar{q}} = \sqrt{|\mathbf{q}|^2+q_4^2} = 1 + * @f] + * + * + * @section jplquat_errorstate Error State Definition + * + * It is important to note that one can prove that the left-multiplicative quaternion error is equivalent to the SO(3) error. + * If one wishes to use the right-hand error, this would need to be implemented as a different type then this class! + * This is noted in Eq. (71) in @cite Trawny2005TR . + * Specifically we have the following: + * \f{align*}{ + * {}^{I}_G\bar{q} &\simeq \begin{bmatrix} \frac{1}{2} \delta \boldsymbol{\theta} \\ 1 \end{bmatrix} \otimes {}^{I}_G\hat{\bar{q}} + * \f} + * which is the same as: + * \f{align*}{ + * {}^{I}_G \mathbf{R} &\simeq \exp(-\delta \boldsymbol{\theta}) {}^{I}_G \hat{\mathbf{R}} \\ + * &\simeq (\mathbf{I} - \lfloor \delta \boldsymbol{\theta} \rfloor) {}^{I}_G \hat{\mathbf{R}} \\ + * \f} + * */ class JPLQuat : public Type { @@ -53,7 +106,7 @@ class JPLQuat : public Type { * quaternion with a quaternion built from a small axis-angle perturbation. * * @f[ - * \bar{q}=norm\Big(\begin{bmatrix} 0.5*\mathbf{\theta_{dx}} \\ 1 \end{bmatrix}\Big) \hat{\bar{q}} + * \bar{q}=norm\Big(\begin{bmatrix} \frac{1}{2} \delta \boldsymbol{\theta}_{dx} \\ 1 \end{bmatrix}\Big) \otimes \hat{\bar{q}} * @f] * * @param dx Axis-angle representation of the perturbing quaternion @@ -73,13 +126,13 @@ class JPLQuat : public Type { /** * @brief Sets the value of the estimate and recomputes the internal rotation matrix - * @param new_value New value for the quaternion estimate + * @param new_value New value for the quaternion estimate (JPL quat as x,y,z,w) */ void set_value(const Eigen::MatrixXd &new_value) override { set_value_internal(new_value); } /** * @brief Sets the fej value and recomputes the fej rotation matrix - * @param new_value New value for the quaternion estimate + * @param new_value New value for the quaternion estimate (JPL quat as x,y,z,w) */ void set_fej(const Eigen::MatrixXd &new_value) override { set_fej_internal(new_value); } diff --git a/ov_core/src/types/Vec.h b/ov_core/src/types/Vec.h index a9bc41903..2725182b0 100644 --- a/ov_core/src/types/Vec.h +++ b/ov_core/src/types/Vec.h @@ -45,6 +45,11 @@ class Vec : public Type { /** * @brief Implements the update operation through standard vector addition + * + * \f{align*}{ + * \mathbf{v} &= \hat{\mathbf{v}} + \tilde{\mathbf{v}}_{dx} + * \f} + * * @param dx Additive error state correction */ void update(const Eigen::VectorXd &dx) override { From 5d6f558467472324aabe0c1a76dce3f0f618fcd7 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 10 May 2023 12:18:10 -0400 Subject: [PATCH 45/60] documentation updates throughout --- ReadMe.md | 3 ++- docs/bib/extra.bib | 29 ++++++++++++++++++++ docs/propagation.dox | 22 ++++++++++++++- ov_core/src/utils/quat_ops.h | 46 +++++++++++++++++++++++++++++++- ov_msckf/launch/serial.launch | 6 ++--- ov_msckf/src/dummy.cpp | 3 ++- ov_msckf/src/state/StateHelper.h | 13 ++++----- 7 files changed, 109 insertions(+), 13 deletions(-) diff --git a/ReadMe.md b/ReadMe.md index d1698022a..13caf9ee7 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -79,6 +79,7 @@ details on what the system supports. * Camera to IMU transform * Camera to IMU time offset * Camera intrinsics + * Inertial intrinsics (including g-sensitivity) * Environmental SLAM feature * OpenCV ARUCO tag SLAM features * Sparse feature SLAM features @@ -90,7 +91,7 @@ details on what the system supports. * Masked tracking * Static and dynamic state initialization * Zero velocity detection and updates -* Out of the box evaluation on EurocMav, TUM-VI, UZH-FPV, KAIST Urban and VIO datasets +* Out of the box evaluation on EuRocMav, TUM-VI, UZH-FPV, KAIST Urban and other VIO datasets * Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..) ## Codebase Extensions diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index 562b1c25d..254dd9ee9 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -1,3 +1,9 @@ +@book{Bar2001, + title = {Estimation with applications to tracking and navigation: theory algorithms and software}, + author = {Bar-Shalom, Yaakov and Li, X Rong and Kirubarajan, Thiagalingam}, + year = 2001, + publisher = {John Wiley \& Sons} +} @book{Barfoot2017, title = {State estimation for robotics}, author = {Barfoot, Timothy D}, @@ -21,6 +27,13 @@ @misc{ceres-solver url = {http://ceres-solver.org}, howpublished = {\url{http://ceres-solver.org}} } +@book{Chatfield1997, + title = {Fundamentals of high accuracy inertial navigation}, + author = {Chatfield, Averil Burton}, + year = 1997, + publisher = {Aiaa}, + volume = 174 +} @inproceedings{Chen2023ICRA, title = {Monocular Visual-Inertial Odometry with Planar Regularities}, author = {Chuchu Chen and Patrick Geneva and Yuxiang Peng and Woosik Lee and Guoquan Huang}, @@ -88,6 +101,12 @@ @article{Eckenhoff2019IJRR doi = {10.1177/0278364919835021}, url = {https://pgeneva.com/downloads/preprints/Eckenhoff2019IJRR.pdf} } +@book{Farrell2008, + title = {Aided navigation: GPS with high rate sensors}, + author = {Farrell, Jay}, + year = 2008, + publisher = {McGraw-Hill, Inc.} +} @inproceedings{Furgale2013IROS, title = {Unified temporal and spatial calibration for multi-sensor systems}, author = {Furgale, Paul and Rehder, Joern and Siegwart, Roland}, @@ -195,6 +214,16 @@ @inproceedings{Li2014ICRA booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, pages = {409--416} } +@article{Li2014IJRR, + title = {Online temporal calibration for camera--IMU systems: Theory and algorithms}, + author = {Li, Mingyang and Mourikis, Anastasios I}, + year = 2014, + journal = {The International Journal of Robotics Research}, + publisher = {SAGE Publications Sage UK: London, England}, + volume = 33, + number = 7, + pages = {947--964} +} @phdthesis{Li2014THESIS, title = {Visual-inertial odometry on resource-constrained systems}, author = {Li, Mingyang}, diff --git a/docs/propagation.dox b/docs/propagation.dox index 66b3084d1..0014e6f96 100644 --- a/docs/propagation.dox +++ b/docs/propagation.dox @@ -45,7 +45,23 @@ the gyroscope and acceleration sensor frames. gyroscope readings due to its inherent inertia. Note that this does not take into account the translation between the gyroscope and accelerometer frames, since it is negligible for most MEMS IMUs (see @cite Li2014ICRA @cite Schneider2019Sensor). -The full background and summary of different models can be found in @cite Yang2020RSS @cite Yang2023TRO. + + + +@m_class{m-block m-warning} + +@par Calibration of Inertial Intrinsics +It is important to note that calibration of inertial intrinsics introduces many additional degrees of freedoms into the state. +Without picking a confident prior distribution for these, the initial trajectory needs to ensure sufficient excitation to enable +calibration and avoid degeneracies. +As found in @cite Yang2020RSS @cite Yang2023TRO there are many trajectories (e.g. planar, 1-axis, constant meas, etc..) +which can hurt inertial intrinsic calibration. +In general we *do not* recommend calibration (but still do it offline!), unless you know your trajectory will +have good excitation (e.g. dynamic handheld). +The full background, summary, and analysis of different inertial models can be found in @cite Yang2020RSS @cite Yang2023TRO. + + + @section ins_state Inertial State Vector @@ -195,6 +211,10 @@ The second is the full continuous-time integration (with constant measurement as - @subpage propagation_discrete --- Simplified discrete mean and covariance propagation derivations - @subpage propagation_analytical --- Analytical mean and covariance propagation with IMU intrinsics derivations +For general background on how to formulate the evolution of states over time we can make a few recommendations. +For classical linear systems we can recommend Maybeck's @cite Maybeck1982STOC Volume 1 and Bar-Shalom et al. @cite Bar2001 discussion of estimator consistency. +For background on Kalman filtering and propagation with similar notation to ours we can recommend Farrell's @cite Farrell2008 aided navigation book. +Finally, for the inertial problem formulation and equations Chatfield's @cite Chatfield1997 fundamental remains foundational. diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index 3eae82bcc..6b1e7c337 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -466,7 +466,18 @@ inline Eigen::Matrix Inv(Eigen::Matrix q) { * * See equation (48) of trawny tech report [Indirect Kalman Filter for 3D Attitude * Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf). + * This matrix is derived in Section 1.5 of the report by finding the Quaterion Time Derivative. * + * \f{align*}{ + * \boldsymbol{\Omega}(\boldsymbol{\omega}) &= + * \begin{bmatrix} + * -\lfloor{\boldsymbol{\omega}} \rfloor & \boldsymbol{\omega} \\ + * -\boldsymbol{\omega}^\top & 0 + * \end{bmatrix} + * \f} + * + * @param w Angular velocity + * @return The matrix \f$\boldsymbol{\Omega}\f$ */ inline Eigen::Matrix Omega(Eigen::Matrix w) { Eigen::Matrix mat; @@ -532,7 +543,7 @@ inline Eigen::Matrix Jr_so3(const Eigen::Matrix &w) * If you are interested in how to compute Jacobians checkout this report: * http://mars.cs.umn.edu/tr/reports/Trawny05c.pdf * - * @param rot Rotation matrix + * @param rot SO(3) rotation matrix * @return roll, pitch, yaw values (in that order) */ inline Eigen::Matrix rot2rpy(const Eigen::Matrix &rot) { @@ -550,7 +561,18 @@ inline Eigen::Matrix rot2rpy(const Eigen::Matrix &ro /** * @brief Construct rotation matrix from given roll + * + * \f{align*}{ + * \mathbf{R}_x(t) &= + * \begin{bmatrix} + * 1 & 0 & 0 \\ + * 0 & \cos(t) & -\sin(t) \\ + * 0 & \sin(t) & \cos(t) + * \end{bmatrix} + * \f} + * * @param t roll angle + * @return SO(3) rotation matrix */ inline Eigen::Matrix rot_x(double t) { Eigen::Matrix r; @@ -562,7 +584,18 @@ inline Eigen::Matrix rot_x(double t) { /** * @brief Construct rotation matrix from given pitch + * + * \f{align*}{ + * \mathbf{R}_y(t) &= + * \begin{bmatrix} + * \cos(t) & 0 & \sin(t) \\ + * 0 & 1 & 0 \\ + * -\sin(t) & 0 & \cos(t) + * \end{bmatrix} + * \f} + * * @param t pitch angle + * @return SO(3) rotation matrix */ inline Eigen::Matrix rot_y(double t) { Eigen::Matrix r; @@ -574,7 +607,18 @@ inline Eigen::Matrix rot_y(double t) { /** * @brief Construct rotation matrix from given yaw + * + * \f{align*}{ + * \mathbf{R}_z(t) &= + * \begin{bmatrix} + * \cos(t) & -\sin(t) & 0 \\ + * \sin(t) & \cos(t) & 0 \\ + * 0 & 0 & 1 + * \end{bmatrix} + * \f} + * * @param t yaw angle + * @return SO(3) rotation matrix */ inline Eigen::Matrix rot_z(double t) { Eigen::Matrix r; diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index a41d40ace..7ce9ed8f9 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -2,16 +2,16 @@ - + - + - + diff --git a/ov_msckf/src/dummy.cpp b/ov_msckf/src/dummy.cpp index 0931284a4..547983c38 100644 --- a/ov_msckf/src/dummy.cpp +++ b/ov_msckf/src/dummy.cpp @@ -37,9 +37,10 @@ * The key features of the system are the following: * * - Sliding stochastic imu clones - * - First estimate Jacobians + * - First-Estimate Jacobians to maintain consistency * - Camera intrinsics and extrinsic online calibration * - Time offset between camera and imu calibration + * - Inertial intrinsic calibration (including g-sensitivity) * - Standard MSCKF features with nullspace projection * - 3d SLAM feature support (with six different representations) * - Generic simulation of trajectories through and environment (see @ref ov_msckf::Simulator) diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index 06b2bee58..6c74013a0 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -82,8 +82,8 @@ class StateHelper { * @param state Pointer to state * @param H_order Variable ordering used in the compressed Jacobian * @param H Condensed Jacobian of updating measurement - * @param res residual of updating measurement - * @param R updating measurement covariance + * @param res Residual of updating measurement + * @param R Updating measurement covariance */ static void EKFUpdate(std::shared_ptr state, const std::vector> &H_order, const Eigen::MatrixXd &H, const Eigen::VectorXd &res, const Eigen::MatrixXd &R); @@ -107,7 +107,7 @@ class StateHelper { * * @param state Pointer to state * @param small_variables Vector of variables whose marginal covariance is desired - * @return marginal covariance of the passed variables + * @return Marginal covariance of the passed variables */ static Eigen::MatrixXd get_marginal_covariance(std::shared_ptr state, const std::vector> &small_variables); @@ -120,7 +120,7 @@ class StateHelper { * Please use the other interface functions in the StateHelper to progamatically change to covariance. * * @param state Pointer to state - * @return covariance of current state + * @return Covariance of current state */ static Eigen::MatrixXd get_full_covariance(std::shared_ptr state); @@ -190,8 +190,9 @@ class StateHelper { * After propagation, normally we augment the state with an new clone that is at the new update timestep. * This augmentation clones the IMU pose and adds it to our state's clone map. * If we are doing time offset calibration we also make our cloning a function of the time offset. - * Time offset logic is based on Mingyang Li and Anastasios I. Mourikis paper: - * http://journals.sagepub.com/doi/pdf/10.1177/0278364913515286 We can write the current clone at the true imu base clock time as the + * Time offset logic is based on Li and Mourikis @cite Li2014IJRR. + * + * We can write the current clone at the true imu base clock time as the * follow: \f{align*}{ * {}^{I_{t+t_d}}_G\bar{q} &= \begin{bmatrix}\frac{1}{2} {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \tilde{t}_d \\ * 1\end{bmatrix}\otimes{}^{I_{t+\hat{t}_d}}_G\bar{q} \\ From f6f0bd163e114f5c5a5c0584eadf5c7a4386cdf3 Mon Sep 17 00:00:00 2001 From: Chu Chu Chen Date: Tue, 9 May 2023 17:03:50 -0400 Subject: [PATCH 46/60] working on fej page --- docs/fej.dox | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/docs/fej.dox b/docs/fej.dox index 8cd6ec891..b2df9f80a 100644 --- a/docs/fej.dox +++ b/docs/fej.dox @@ -4,7 +4,7 @@ @page fej First Estimate Jacobian (FEJ) Estimation @tableofcontents - + The observability and consistency of VINS have attracted significant research efforts due to its ability to provide: (i) the minimal conditions for initialization, (ii) insights into what states are unrecoverable, and (iii) degenerate motions which can hurt the performance of the system. However, EKF-based VINS estimators have been shown to be inconsistent due to spurious information gain along unobservable directions and have required the creation of “observability aware” filters which explicitly correct for these inaccurate information gains causing filter over-confidence. In this section, we will introduce First-estimates Jacobian (FEJ) methodology which can guarantee the observability properties of VINS. @section fej-sys-evolution EKF Linearized Error-State System @@ -101,7 +101,10 @@ while we here are interested in how the state evolves in order to examine it @section fej-sys-observability Linearized System Observability -The observability matrix of this linearized system is defined by: +System observability plays a crucial role in state estimation. +Understanding system observability provides a deep insight about the system’s geometrical properties and determines the minimal +measurement modalities needed. +With the state translation matrix, \f$\mathbf{\Phi}_{(k,0)}\f$, and measurement Jacobian at timestep *k*, \f$\mathbf{H}_{k}\f$ , the observability matrix of this linearized system is defined by: \f{align*}{ \mathcal{O}= @@ -114,7 +117,7 @@ The observability matrix of this linearized system is defined by: \end{bmatrix} \f} -where \f$\mathbf{H}_{k}\f$ is the measurement Jacobian at timestep *k* and \f$\mathbf{\Phi}_{(k,0)}\f$ is the compounded state transition (system Jacobian) matrix from timestep 0 to k. +If \f$\mathcal{O}\f$ is of full column rank, the system is fully observable. For a given block row of this matrix, we have: \f{align*}{ @@ -141,7 +144,7 @@ For a given block row of this matrix, we have: \f} -We now verify the following nullspace which corresponds to the global yaw about gravity and global IMU and feature positions: +We now verify the following nullspace which corresponds to the global yaw about gravity and global IMU and feature positions: \f{align*}{ @@ -155,7 +158,7 @@ We now verify the following nullspace which corresponds to the global yaw about \f} It is not difficult to verify that \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N}_{vio} = \mathbf 0 \f$. -Thus this is a nullspace of the system, +Thus this is a nullspace of the system, which clearly shows that there are the four unobserable directions (global yaw and position) of visual-inertial systems. @@ -163,6 +166,7 @@ which clearly shows that there are the four unobserable directions (global yaw a @section fej-fej First Estimate Jacobians + The main idea of First-Estimate Jacobains (FEJ) approaches is to ensure that the state transition and Jacobian matrices are evaluated at correct linearization points such that the above observability analysis will hold true. For those interested in the technical details please take a look at: @cite Huang2010IJRR and @cite Li2013IJRR. Let us first consider a small thought experiment of how the standard Kalman filter computes its state transition matrix. @@ -203,4 +207,4 @@ For example if we evaluated the \f$\mathbf H_k \f$ Jacobian with a different \f$ - */ \ No newline at end of file + */ From 97aae2e28f34f8c8508057179f08a9443ebd87d6 Mon Sep 17 00:00:00 2001 From: Chu Chu Chen Date: Wed, 10 May 2023 12:17:55 -0400 Subject: [PATCH 47/60] improve FEJ doc --- docs/bib/extra.bib | 7 +++ docs/fej.dox | 143 ++++++++++++++++++++++++++------------------- 2 files changed, 89 insertions(+), 61 deletions(-) diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index 254dd9ee9..faf19b32e 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -41,6 +41,13 @@ @inproceedings{Chen2023ICRA booktitle = {2023 International Conference on Robotics and Automation (ICRA)}, url = {https://pgeneva.com/downloads/papers/Chen2023ICRA.pdf} } +@inproceedings{Chen2022ICRA, + author = {Chuchu Chen and Yulin Yang and Patrick Geneva and Guoquan Huang}, + title = {FEJ2: A Consistent Visual-Inertial State Estimator Design}, + year = 2022, + booktitle = {2022 International Conference on Robotics and Automation (ICRA)}, + url = {https://chuchuchen.net/downloads/papers/Chen2022ICRA.pdf} +} @book{Chirikjian2011, title = {Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications}, author = {Chirikjian, Gregory}, diff --git a/docs/fej.dox b/docs/fej.dox index b2df9f80a..73e529fb4 100644 --- a/docs/fej.dox +++ b/docs/fej.dox @@ -4,22 +4,18 @@ @page fej First Estimate Jacobian (FEJ) Estimation @tableofcontents - The observability and consistency of VINS have attracted significant research efforts due to its ability to provide: (i) the minimal conditions for initialization, (ii) insights into what states are unrecoverable, and (iii) degenerate motions which can hurt the performance of the system. However, EKF-based VINS estimators have been shown to be inconsistent due to spurious information gain along unobservable directions and have required the creation of “observability aware” filters which explicitly correct for these inaccurate information gains causing filter over-confidence. In this section, we will introduce First-estimates Jacobian (FEJ) methodology which can guarantee the observability properties of VINS. +The observability and consistency of VINS is crutial due to its ability to provide: (i) the minimal conditions for initialization, (ii) insights into what states are unrecoverable, and (iii) degenerate motions which can hurt the performance of the system. However, EKF-based VINS estimators have been shown to be inconsistent due to spurious information gain along unobservable directions and have required the creation of “observability aware” filters which explicitly correct for these inaccurate information gains causing filter over-confidence (its uncertainty is smaller than the true). In this section, we will introduce First-estimates Jacobian (FEJ) methodology which can guarantee the observability properties of VINS and improve the estimation consistency. -@section fej-sys-evolution EKF Linearized Error-State System - -When developing an extended Kalman filter (EKF), one needs to linearize the nonlinear motion and measurement models about some linearization point. -This linearization is one of the sources of error causing inaccuracies in the estimates (in addition to, for exmaple, model errors and measurement noise). -Let us consider the following linearized error-state visual-inertial system: +> Since the filter gain is based on the filter-calculated error covariances, it follows that consistency is necessary for filter optimality: Wrong covariance yield wrong gain. +> This is why consistency evaluation is vital for verifying a filter design -- it amounts to evaluation of estimator optimality. -\f{align*}{ -\tilde{\mathbf{x}}_{k|k-1} &= \mathbf{\Phi}_{(k,k-1)}~\tilde{\mathbf{x}}_{k-1|k-1} + \mathbf{G}_{k}\mathbf{w}_{k} \\ -\tilde{\mathbf{z}}_{k} &= \mathbf{H}_{k}~\tilde{\mathbf{x}}_{k|k-1}+\mathbf{n}_{k} -\f} -where the state contains the inertial navigation state and a single environmental feature -(noting that we do not include biases to simplify the derivations): +@section fej-sys-evolution EKF Linearized Error-State System +When developing an extended Kalman filter (EKF), one needs to linearize the nonlinear motion and measurement models about some linearization points. +This linearization is one of the sources of error causing inaccuracies in the estimates (in addition to, for exmaple, model errors and measurement noise). +To simplify the derivations, in what follows, we consider the state contains the inertial navigation state and a single environmental feature (no biases). +We refer the reader to @cite Huang2010IJRR @cite Hesch2013TRO @cite Hesch2014IJRR @cite Chen2022ICRA for more detailed derivations of the full state variables. \f{align*}{ \mathbf{x}_k &= \begin{bmatrix} @@ -30,40 +26,17 @@ where the state contains the inertial navigation state and a single environmenta \end{bmatrix}^{\top} \f} -Note that we use the left quaternion error state -(see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR for details). -For simplicity we assume that the camera and IMU frame have an identity transform. -We can compute the measurement Jacobian of a given feature based on the perspective projection camera model -at the *k*-th timestep as follows: + +Let us consider the following simplified linearized error-state visual-inertial system with the IMU kinematic motion model and camera measurement update: \f{align*}{ -\mathbf{H}_{k} &= -\mathbf H_{proj,k}~\mathbf H_{state,k} \\ -&= -\begin{bmatrix} -\frac{1}{{}^Iz} & 0 & \frac{-{}^Ix}{({}^Iz)^2} \\ -0 & \frac{1}{{}^Iz} & \frac{-{}^Iy}{({}^Iz)^2} \\ -\end{bmatrix} -\begin{bmatrix} -\lfloor {}^{I_k}_{G}\mathbf{R}({}^{G}\mathbf{p}_f-{}^{G}\mathbf{p}_{I_k}) \times\rfloor & --{}^{I_k}_{G}\mathbf{R} & -\mathbf 0_{3\times3} & -{}^{I_k}_{G}\mathbf{R} -\end{bmatrix} \\ -&= -\mathbf H_{proj,k}~ -{}^{I_k}_{G}\mathbf{R} -\begin{bmatrix} -\lfloor ({}^{G}\mathbf{p}_f-{}^{G}\mathbf{p}_{I_k}) \times\rfloor {}^{I_k}_{G}\mathbf{R}^\top & --\mathbf I_{3\times3} & -\mathbf 0_{3\times3} & -\mathbf I_{3\times3} -\end{bmatrix} +\tilde{\mathbf{x}}_{k|k-1} &= \mathbf{\Phi}_{(k,k-1)}~\tilde{\mathbf{x}}_{k-1|k-1} + \mathbf{G}_{k}\mathbf{w}_{k} \\ +\tilde{\mathbf{z}}_{k} &= \mathbf{H}_{k}~\tilde{\mathbf{x}}_{k|k-1}+\mathbf{n}_{k} \f} - -The state-transition (or system Jacobian) matrix from timestep *k-1* to *k* as (see [@ref propagation] for more details): - +Note that we use the left quaternion error state (see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR for details). +For simplicity we assume that the camera and IMU frame have an identity transform. +The state-transition (or system Jacobian) matrix from timestep *k-1* to *k* can be derived as (see [@ref propagation] for more details): \f{align*}{ \mathbf{\Phi}_{(k,k-1)}&= @@ -95,14 +68,40 @@ The state-transition (or system Jacobian) matrix from timestep *k-1* to *k* as ( where \f$\mathbf a(\tau)\f$ is the true acceleration at time \f$\tau \f$, \f${}^{I_{k}}_{I_{k-1}}\mathbf R\f$ is computed using the gyroscope angular velocity measurements, and \f${}^{G}\mathbf g = [0 ~ 0 ~ 9.81]^\top\f$ is gravity in the global frame of reference. During propagation one would need to solve these integrals using either analytical or numerical integration, -while we here are interested in how the state evolves in order to examine its observability. +We can compute the measurement Jacobian of a given feature based on the perspective projection camera model +at the *k*-th timestep as follows (see [@ref update-feat] for more details): + +\f{align*}{ +\mathbf{H}_{k} &= +\mathbf H_{proj,k}~\mathbf H_{state,k} \\ +&= +\begin{bmatrix} +\frac{1}{{}^Iz} & 0 & \frac{-{}^Ix}{({}^Iz)^2} \\ +0 & \frac{1}{{}^Iz} & \frac{-{}^Iy}{({}^Iz)^2} \\ +\end{bmatrix} +\begin{bmatrix} +\lfloor {}^{I_k}_{G}\mathbf{R}({}^{G}\mathbf{p}_f-{}^{G}\mathbf{p}_{I_k}) \times\rfloor & +-{}^{I_k}_{G}\mathbf{R} & +\mathbf 0_{3\times3} & +{}^{I_k}_{G}\mathbf{R} +\end{bmatrix} \\ +&= +\mathbf H_{proj,k}~ +{}^{I_k}_{G}\mathbf{R} +\begin{bmatrix} +\lfloor ({}^{G}\mathbf{p}_f-{}^{G}\mathbf{p}_{I_k}) \times\rfloor {}^{I_k}_{G}\mathbf{R}^\top & +-\mathbf I_{3\times3} & +\mathbf 0_{3\times3} & +\mathbf I_{3\times3} +\end{bmatrix} +\f} + @section fej-sys-observability Linearized System Observability -System observability plays a crucial role in state estimation. -Understanding system observability provides a deep insight about the system’s geometrical properties and determines the minimal +System observability plays a crucial role in state estimation since it provides a deep insight about the system’s geometrical properties and determines the minimal measurement modalities needed. With the state translation matrix, \f$\mathbf{\Phi}_{(k,0)}\f$, and measurement Jacobian at timestep *k*, \f$\mathbf{H}_{k}\f$ , the observability matrix of this linearized system is defined by: @@ -117,7 +116,8 @@ With the state translation matrix, \f$\mathbf{\Phi}_{(k,0)}\f$, and measurement \end{bmatrix} \f} -If \f$\mathcal{O}\f$ is of full column rank, the system is fully observable. +If \f$\mathcal{O}\f$ is of full column rank the system is fully observable and its nullspace (i.e., \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} = \mathbf 0 \f$) describes the state unobservable subspace can not be recovered with given measurements. +Note that we only consider the block row of the observability matrix when performing observablity analysis is due to the fact that the analysis hold the same for the entire matrix (i.e., each block row share the same nullspace). For a given block row of this matrix, we have: \f{align*}{ @@ -143,12 +143,10 @@ For a given block row of this matrix, we have: \boldsymbol\Gamma_4 &= \mathbf I_{3\times3} \f} - -We now verify the following nullspace which corresponds to the global yaw about gravity and global IMU and feature positions: - +It is straightforward to verify the right nullspace spans four directions, i.e.,: \f{align*}{ - \mathcal{N}_{vins} &= + \mathcal{N} &= \begin{bmatrix} {}^{I_{0}}_{G}\mathbf{R}{}^G\mathbf{g} & \mathbf 0_{3\times3} \\ -\lfloor {}^{G}\mathbf{p}_{I_0} \times\rfloor{}^G\mathbf{g} & \mathbf{I}_{3\times3} \\ @@ -156,19 +154,19 @@ We now verify the following nullspace which corresponds to the global yaw about -\lfloor {}^{G}\mathbf{p}_{f} \times\rfloor{}^G\mathbf{g} & \mathbf{I}_{3\times3} \\ \end{bmatrix} \f} +where \f$ \mathcal{N} \f$ should be 4dof corrsponds to global translation and global rotation about the gravity (yaw) of visual-inertial systems. -It is not difficult to verify that \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N}_{vio} = \mathbf 0 \f$. -Thus this is a nullspace of the system, -which clearly shows that there are the four unobserable directions (global yaw and position) of visual-inertial systems. +@section fej-fej First Estimate Jacobians +It has been showed that standard EKF based-VINS, which always computes the state translation matrix and the measurement Jacobian using the current state estimates make the global orientation apperas to be observable and reduce the nullspace to only 3dof @cite Hesch2013TRO. This causes the filter mistakenly gain \textit{extra} information and become overconfident with estimation results. +To solve this issue, First-Estimate Jacobains (FEJ) approach guarantee the observability properties by evaluate the linearized system state transition matrix and Jacobians at the same estimate (i.e., the first estimates) over all time periods, thus ensuring that the 4dof unobservable VINS subspace does not gain spurious information. +The application of FEJ is simple yet efficitive, let us consider the propogation and update function. -@section fej-fej First Estimate Jacobians + @subsection Propagation -The main idea of First-Estimate Jacobains (FEJ) approaches is to ensure that the state transition and Jacobian matrices are evaluated at correct linearization points such that the above observability analysis will hold true. -For those interested in the technical details please take a look at: @cite Huang2010IJRR and @cite Li2013IJRR. Let us first consider a small thought experiment of how the standard Kalman filter computes its state transition matrix. From a timestep zero to one it will use the current estimates from state zero forward in time. At the next timestep after it updates the state with measurements from other sensors, it will compute the state transition with the updated values to evolve the state to timestep two. @@ -198,13 +196,36 @@ To fix this, we can change the linearization point we evaluate these at: \f} -We also need to ensure that our measurement Jacobians match the linearization point of the state transition matrix. -Thus they also need to be evaluated at the \f$\mathbf{x}_{k|k-1}\f$ linearization point instead of the \f$\mathbf{x}_{k|k}\f$ that one would normally use. -This gives way to the name FEJ since we will evaluate the Jacobians at the same linearization point to ensure that the nullspace remains valid. -For example if we evaluated the \f$\mathbf H_k \f$ Jacobian with a different \f$ {}^G\mathbf{p}_f \f$ at each timestep then the nullspace would not hold past the first time instance. - +@subsection Update +We also need to ensure that our measurement Jacobians match the linearization point of the state transition matrix. +Let us consider the simple case, at timestamp k the current state estimate is denoted as \f$ \hat{\mathbf{x}}_k \f$. +Take a look of the system observability matrix: +\f{align*}{ +\hat{\mathcal{O}}= +\begin{bmatrix} +\hat{\mathbf{H}}_{0}\mathbf{\Phi}_{(0,0)} \\ +\vdots \\ +\hat{\mathbf{H}}_{k}\mathbf{\Phi}_{(k,0)} \\ +\end{bmatrix} +\f} +It is easy to veriy that \f$ \mathcal{N} \f$ we derived from the previous section does not satify the entire matrix (i.e., \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} \neq \mathbf 0 \f$). +Also, the dimension of unobservable subspace becomes 3 (i.e., \f$ \texttt{dim}(\hat{\mathcal{O}}) =3 \f$). +This will causes the filter gain extra and non-exit information along the unobservable direction and hurt the estimation performance, it can lead to larger errors, erroneously smaller uncertainties, and inconsistency (See @cite Hesch2013TRO for detailed proof and discussion). +To assure \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} = \mathbf 0 \f$, a nartual choice of state linearizatin point is to use the first state estimate \f$ \bar{\mathbf{x}}_k \f$. +Note that for the IMU state, its first state estimate will be \f$ {\mathbf{x}}_{k|k-1}\f$ which also match the linearization point in the state transition matrix. +For the features, we adopt the triangulated value (i.e., at the time we initialize the feature into state vector) as its first estimate. +As such, we can derive the lineairzed measurement update function as: +\f{align*}{ + \tilde{\mathbf{z}}_{k+1} = \mathbf{z}_{k+1} - \mathbf{h}(\hat{\mathbf{x}}_k) \simeq \bar{\mathbf{H}}_{k} (\mathbf{x}_{k} -\hat{\mathbf{x}}_k ) + \mathbf{n}_k +\f} +where \f$ \mathbf{h}(\cdot) \f$ is the nonlinear measurement function and \f$ \bar{\mathbf{H}}_{k} \f$ is the first estimate Jacobian which only consist the first state estimates \f$ \bar{\mathbf{x}}_k \f$. +We will then applied the standard EKF update equarions to perform state update using the above linearized measurement function. +It is not difficult to evaluate that one can construct the observability matrix \f$ \mathcal{O} \f$ with the FEJ state state transition matrix and the measurement Jacobian to yeild a valid (4dof) unobservable nullspace. +As such, the system observability properities can be preserved. While not utilizing the current state estimation for linearization may go against intuition, FEJ has proven to enhance the accuracy and consistency of VINS in many cases. +FEJ's use of the first state estimates may introduce of "unmodeled" error into the filter, which is a potential issue to keep in mind. +This problem has been discussed and address in our follow up work FEJ2 @cite Chen2022ICRA and OC @cite Huang2010IJRR @cite Hesch2013TRO @cite Hesch2014IJRR. For interested readers, we recommend exploring these follow-up studies. */ From d37dfe2660f6069e49079f64a183715adbabe0b6 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 10 May 2023 13:11:48 -0400 Subject: [PATCH 48/60] cleanup fej doc page wording --- docs/bib/extra.bib | 33 ++++++++++--- docs/fej.dox | 121 +++++++++++++++++++++++++++++---------------- 2 files changed, 105 insertions(+), 49 deletions(-) diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index faf19b32e..3e5879090 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -34,6 +34,13 @@ @book{Chatfield1997 publisher = {Aiaa}, volume = 174 } +@inproceedings{Chen2022ICRA, + title = {FEJ2: A Consistent Visual-Inertial State Estimator Design}, + author = {Chuchu Chen and Yulin Yang and Patrick Geneva and Guoquan Huang}, + year = 2022, + booktitle = {2022 International Conference on Robotics and Automation (ICRA)}, + url = {https://chuchuchen.net/downloads/papers/Chen2022ICRA.pdf} +} @inproceedings{Chen2023ICRA, title = {Monocular Visual-Inertial Odometry with Planar Regularities}, author = {Chuchu Chen and Patrick Geneva and Yuxiang Peng and Woosik Lee and Guoquan Huang}, @@ -41,13 +48,6 @@ @inproceedings{Chen2023ICRA booktitle = {2023 International Conference on Robotics and Automation (ICRA)}, url = {https://pgeneva.com/downloads/papers/Chen2023ICRA.pdf} } -@inproceedings{Chen2022ICRA, - author = {Chuchu Chen and Yulin Yang and Patrick Geneva and Guoquan Huang}, - title = {FEJ2: A Consistent Visual-Inertial State Estimator Design}, - year = 2022, - booktitle = {2022 International Conference on Robotics and Automation (ICRA)}, - url = {https://chuchuchen.net/downloads/papers/Chen2022ICRA.pdf} -} @book{Chirikjian2011, title = {Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications}, author = {Chirikjian, Gregory}, @@ -148,6 +148,25 @@ @article{Hesch2013TRO number = 1, pages = {158--176} } +@article{Hesch2014IJRR, + title = {Camera-IMU-based localization: Observability analysis and consistency improvement}, + author = {Hesch, Joel A and Kottas, Dimitrios G and Bowman, Sean L and Roumeliotis, Stergios I}, + year = 2014, + journal = {The International Journal of Robotics Research}, + publisher = {SAGE Publications Sage UK: London, England}, + volume = 33, + number = 1, + pages = {182--201} +} +@inproceedings{Huang2009ISER, + title = {A first-estimates Jacobian EKF for improving SLAM consistency}, + author = {Huang, Guoquan P and Mourikis, Anastasios I and Roumeliotis, Stergios I}, + year = 2009, + booktitle = {Experimental Robotics: The Eleventh International Symposium}, + pages = {373--382}, + url = {https://intra.ece.ucr.edu/~mourikis/papers/Huang08-ISER.pdf}, + organization = {Springer} +} @article{Huang2010IJRR, title = {Observability-based rules for designing consistent EKF SLAM estimators}, author = {Huang, Guoquan P and Mourikis, Anastasios I and Roumeliotis, Stergios I}, diff --git a/docs/fej.dox b/docs/fej.dox index 73e529fb4..2722fafb3 100644 --- a/docs/fej.dox +++ b/docs/fej.dox @@ -4,18 +4,30 @@ @page fej First Estimate Jacobian (FEJ) Estimation @tableofcontents -The observability and consistency of VINS is crutial due to its ability to provide: (i) the minimal conditions for initialization, (ii) insights into what states are unrecoverable, and (iii) degenerate motions which can hurt the performance of the system. However, EKF-based VINS estimators have been shown to be inconsistent due to spurious information gain along unobservable directions and have required the creation of “observability aware” filters which explicitly correct for these inaccurate information gains causing filter over-confidence (its uncertainty is smaller than the true). In this section, we will introduce First-estimates Jacobian (FEJ) methodology which can guarantee the observability properties of VINS and improve the estimation consistency. +The observability and consistency of VINS is crucial due to its ability to provide: +(i) the minimal conditions for initialization, +(ii) insights into what states are unrecoverable, and +(iii) degenerate motions which can hurt the performance of the system. +Naive EKF-based VINS estimators have been shown to be inconsistent due to spurious information gain along unobservable directions +and have required the creation of "observability aware" filters which explicitly address the inaccurate information gains causing +filter over-confidence (the estimated uncertainty is smaller than the true). +Bar-Shalom et al. @cite Bar2001 make a convincing argument of why estimator consistency is crucial for accuracy and robust estimation: > Since the filter gain is based on the filter-calculated error covariances, it follows that consistency is necessary for filter optimality: Wrong covariance yield wrong gain. -> This is why consistency evaluation is vital for verifying a filter design -- it amounts to evaluation of estimator optimality. +> This is why consistency evaluation is vital for verifying a filter design -- *it amounts to evaluation of estimator optimality*. + + +In this section, we will introduce First-estimates Jacobian (FEJ) @cite Huang2009ISER @cite Huang2010IJRR methodology which can guarantee +the observability properties of VINS and improve the estimation consistency. + @section fej-sys-evolution EKF Linearized Error-State System -When developing an extended Kalman filter (EKF), one needs to linearize the nonlinear motion and measurement models about some linearization points. -This linearization is one of the sources of error causing inaccuracies in the estimates (in addition to, for exmaple, model errors and measurement noise). +When developing an Extended Kalman filter (EKF), one needs to linearize the nonlinear motion and measurement models about some linearization points. +This linearization is one of the sources of error causing inaccuracies in the estimates (in addition to, for example, model errors and measurement noise). To simplify the derivations, in what follows, we consider the state contains the inertial navigation state and a single environmental feature (no biases). -We refer the reader to @cite Huang2010IJRR @cite Hesch2013TRO @cite Hesch2014IJRR @cite Chen2022ICRA for more detailed derivations of the full state variables. + \f{align*}{ \mathbf{x}_k &= \begin{bmatrix} @@ -26,7 +38,7 @@ We refer the reader to @cite Huang2010IJRR @cite Hesch2013TRO @cite Hesch2014IJR \end{bmatrix}^{\top} \f} - +We refer the reader to @cite Huang2010IJRR @cite Li2013IJRR @cite Hesch2013TRO @cite Hesch2014IJRR @cite Chen2022ICRA for more detailed derivations of the full state variables. Let us consider the following simplified linearized error-state visual-inertial system with the IMU kinematic motion model and camera measurement update: \f{align*}{ @@ -34,7 +46,7 @@ Let us consider the following simplified linearized error-state visual-inertial \tilde{\mathbf{z}}_{k} &= \mathbf{H}_{k}~\tilde{\mathbf{x}}_{k|k-1}+\mathbf{n}_{k} \f} -Note that we use the left quaternion error state (see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR for details). +Note that we use the left quaternion error state (see [[Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf)] @cite Trawny2005TR and @ref ov_type::JPLQuat for details). For simplicity we assume that the camera and IMU frame have an identity transform. The state-transition (or system Jacobian) matrix from timestep *k-1* to *k* can be derived as (see [@ref propagation] for more details): @@ -66,9 +78,9 @@ The state-transition (or system Jacobian) matrix from timestep *k-1* to *k* can \f} -where \f$\mathbf a(\tau)\f$ is the true acceleration at time \f$\tau \f$, \f${}^{I_{k}}_{I_{k-1}}\mathbf R\f$ is computed using the gyroscope angular velocity measurements, and \f${}^{G}\mathbf g = [0 ~ 0 ~ 9.81]^\top\f$ is gravity in the global frame of reference. -During propagation one would need to solve these integrals using either analytical or numerical integration, - +where \f$\mathbf a(\tau)\f$ is the true acceleration at time \f$\tau \f$, \f${}^{I_{k}}_{I_{k-1}}\mathbf R\f$ is computed +using the gyroscope angular velocity measurements, and \f${}^{G}\mathbf g = [0 ~ 0 ~ 9.81]^\top\f$ is gravity in the global frame of reference. +During propagation one would need to solve these integrals using either analytical or numerical integration. We can compute the measurement Jacobian of a given feature based on the perspective projection camera model at the *k*-th timestep as follows (see [@ref update-feat] for more details): @@ -99,11 +111,14 @@ at the *k*-th timestep as follows (see [@ref update-feat] for more details): \f} + + @section fej-sys-observability Linearized System Observability -System observability plays a crucial role in state estimation since it provides a deep insight about the system’s geometrical properties and determines the minimal -measurement modalities needed. -With the state translation matrix, \f$\mathbf{\Phi}_{(k,0)}\f$, and measurement Jacobian at timestep *k*, \f$\mathbf{H}_{k}\f$ , the observability matrix of this linearized system is defined by: +System observability plays a crucial role in state estimation since it provides a deep insight about the system's +geometrical properties and determines the minimal measurement modalities needed. +With the state translation matrix, \f$\mathbf{\Phi}_{(k,0)}\f$, and measurement Jacobian at timestep *k*, +\f$\mathbf{H}_{k}\f$, the observability matrix of this linearized system is defined by: \f{align*}{ \mathcal{O}= @@ -116,9 +131,13 @@ With the state translation matrix, \f$\mathbf{\Phi}_{(k,0)}\f$, and measurement \end{bmatrix} \f} -If \f$\mathcal{O}\f$ is of full column rank the system is fully observable and its nullspace (i.e., \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} = \mathbf 0 \f$) describes the state unobservable subspace can not be recovered with given measurements. -Note that we only consider the block row of the observability matrix when performing observablity analysis is due to the fact that the analysis hold the same for the entire matrix (i.e., each block row share the same nullspace). -For a given block row of this matrix, we have: +If \f$\mathcal{O}\f$ is of full column rank then the system is fully observable. +A nullspace of it (i.e., \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} = \mathbf 0 \f$) describes the unobservable +state subspace which can not be recovered with given measurements. +Note that while we simplify here and only consider the block row of the observability matrix when performing observability analysis, +we need to ensure that this nullsapce holds for the entire matrix (i.e., each block row share the same nullspace). +This can be achieved by ensuring that the nullspace does not vary with time, nor contain any measurements. +For the *k*-th block row of this \f$\mathcal{O}\f$ matrix, we have: \f{align*}{ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)} &= @@ -154,41 +173,49 @@ It is straightforward to verify the right nullspace spans four directions, i.e., -\lfloor {}^{G}\mathbf{p}_{f} \times\rfloor{}^G\mathbf{g} & \mathbf{I}_{3\times3} \\ \end{bmatrix} \f} -where \f$ \mathcal{N} \f$ should be 4dof corrsponds to global translation and global rotation about the gravity (yaw) of visual-inertial systems. + +where \f$ \mathcal{N} \f$ should be 4dof corresponding to global rotation about the gravity (yaw) and global translation of our visual-inertial systems. + + + @section fej-fej First Estimate Jacobians -It has been showed that standard EKF based-VINS, which always computes the state translation matrix and the measurement Jacobian using the current state estimates make the global orientation apperas to be observable and reduce the nullspace to only 3dof @cite Hesch2013TRO. This causes the filter mistakenly gain \textit{extra} information and become overconfident with estimation results. -To solve this issue, First-Estimate Jacobains (FEJ) approach guarantee the observability properties by evaluate the linearized system state transition matrix and Jacobians at the same estimate (i.e., the first estimates) over all time periods, thus ensuring that the 4dof unobservable VINS subspace does not gain spurious information. -The application of FEJ is simple yet efficitive, let us consider the propogation and update function. +It has been showed that standard EKF based-VINS, which always computes the state translation matrix and the measurement +Jacobian using the current state estimates, has the global yaw orientation appear to be observable +and has an incorrectly reduced 3dof nullspace dimention @cite Hesch2013TRO. +This causes the filter mistakenly *gaining extra information* and becoming overconfident in the yaw. +To solve this issue, the First-Estimate Jacobains (FEJ) @cite Huang2009ISER @cite Huang2010IJRR methodology can be applied. +It evaluates the linearized system state transition matrix and Jacobians at the same estimate (i.e., the first estimates) +over all time periods and thus ensures that the 4dof unobservable VINS subspace does not gain spurious information. +The application of FEJ is simple yet effective, let us consider how to modify the propagation and update linearizations. - @subsection Propagation + +@subsection Propagation Let us first consider a small thought experiment of how the standard Kalman filter computes its state transition matrix. From a timestep zero to one it will use the current estimates from state zero forward in time. At the next timestep after it updates the state with measurements from other sensors, it will compute the state transition with the updated values to evolve the state to timestep two. This causes a miss-match in the "continuity" of the state transition matrix which when multiply sequentially should represent the evolution from time zero to time two. - \f{align*}{ \mathbf{\Phi}_{(k+1,k-1)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k-1|k-1}) \neq \mathbf{\Phi}_{(k+1,k)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k|k}) ~ \mathbf{\Phi}_{(k,k-1)}(\mathbf{x}_{k|k-1},\mathbf{x}_{k-1|k-1}) \f} - As shown above, we wish to compute the state transition matrix from the *k-1* timestep given all *k-1* measurements up until the current propagated timestep *k+1* given all *k* measurements. The right side of the above equation is how one would normally perform this in a Kalman filter framework. \f$\mathbf{\Phi}_{(k,k-1)}(\mathbf{x}_{k|k-1},\mathbf{x}_{k-1|k-1})\f$ corresponds to propagating from the *k-1* update time to the *k* timestep. -One would then normally perform the *k*'th update to the state and then propagate from this **updated** state to the newest timestep (i.e. the \f$ \mathbf{\Phi}_{(k+1,k)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k|k}) \f$ state transition matrix). -This clearly is different then if one was to compute the state transition from time *k-1* to the *k+1* timestep as the second state transition is evaluated at the different \f$\mathbf{x}_{k|k}\f$ linearization point! +One would then normally perform the *k*'th update to the state and then propagate from this **updated** state to the newest timestep +(i.e. the \f$ \mathbf{\Phi}_{(k+1,k)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k|k}) \f$ state transition matrix). +This clearly is different then if one was to compute the state transition from time *k-1* to the *k+1* timestep as +the second state transition is evaluated at the different \f$\mathbf{x}_{k|k}\f$ linearization point! To fix this, we can change the linearization point we evaluate these at: - - \f{align*}{ \mathbf{\Phi}_{(k+1,k-1)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k-1|k-1}) = \mathbf{\Phi}_{(k+1,k)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k|k-1}) ~ @@ -198,9 +225,10 @@ To fix this, we can change the linearization point we evaluate these at: @subsection Update + We also need to ensure that our measurement Jacobians match the linearization point of the state transition matrix. -Let us consider the simple case, at timestamp k the current state estimate is denoted as \f$ \hat{\mathbf{x}}_k \f$. -Take a look of the system observability matrix: +Let us consider the simple case at timestamp *k* with current state estimate +\f$\hat{\mathbf{x}}_k\f$ gives the following observability matrix: \f{align*}{ \hat{\mathcal{O}}= @@ -210,22 +238,31 @@ Take a look of the system observability matrix: \hat{\mathbf{H}}_{k}\mathbf{\Phi}_{(k,0)} \\ \end{bmatrix} \f} -It is easy to veriy that \f$ \mathcal{N} \f$ we derived from the previous section does not satify the entire matrix (i.e., \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} \neq \mathbf 0 \f$). -Also, the dimension of unobservable subspace becomes 3 (i.e., \f$ \texttt{dim}(\hat{\mathcal{O}}) =3 \f$). -This will causes the filter gain extra and non-exit information along the unobservable direction and hurt the estimation performance, it can lead to larger errors, erroneously smaller uncertainties, and inconsistency (See @cite Hesch2013TRO for detailed proof and discussion). -To assure \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} = \mathbf 0 \f$, a nartual choice of state linearizatin point is to use the first state estimate \f$ \bar{\mathbf{x}}_k \f$. +It is easy to verify that \f$ \mathcal{N} \f$, derived from the previous section, is not valid for the whole entire matrix (i.e., \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} \neq \mathbf 0 \f$). +For example, the feature estimate \f$ {}^{G}\hat{\mathbf{p}}_{f}(t_0) \neq ... \neq {}^{G}\hat{\mathbf{p}}_{f}(t_k) \f$ in each row of \f$ \hat{\mathcal{O}}\f$ are different, thus the nullsace does not hold. +Specifically the first column \f$ \mathcal{N} \f$ is invalid, and causes the dimension of unobservable subspace to become 3 (i.e., \f$ \texttt{dim}(\hat{\mathcal{O}}) =3 \f$). +This will cause the filter to gain extra information along the yaw unobservable direction and hurt the estimation performance. +Specifically this leads to larger errors, erroneously smaller uncertainties, and inconsistency (see @cite Hesch2013TRO @cite Li2013IJRR for detailed proof and discussion). + +To assure \f$ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)}\mathcal{N} = \mathbf 0 \f$, one can fix the linearization point over all time. +A natural choice of state linearization point is to use the first state estimate \f$ \bar{\mathbf{x}}_k \f$. Note that for the IMU state, its first state estimate will be \f$ {\mathbf{x}}_{k|k-1}\f$ which also match the linearization point in the state transition matrix. -For the features, we adopt the triangulated value (i.e., at the time we initialize the feature into state vector) as its first estimate. -As such, we can derive the lineairzed measurement update function as: +For the features, the initial triangulated value (i.e., at the time we initialize the feature into state vector) is its first estimate. +As such, we can derive the linearized measurement update function as: \f{align*}{ \tilde{\mathbf{z}}_{k+1} = \mathbf{z}_{k+1} - \mathbf{h}(\hat{\mathbf{x}}_k) \simeq \bar{\mathbf{H}}_{k} (\mathbf{x}_{k} -\hat{\mathbf{x}}_k ) + \mathbf{n}_k \f} -where \f$ \mathbf{h}(\cdot) \f$ is the nonlinear measurement function and \f$ \bar{\mathbf{H}}_{k} \f$ is the first estimate Jacobian which only consist the first state estimates \f$ \bar{\mathbf{x}}_k \f$. -We will then applied the standard EKF update equarions to perform state update using the above linearized measurement function. -It is not difficult to evaluate that one can construct the observability matrix \f$ \mathcal{O} \f$ with the FEJ state state transition matrix and the measurement Jacobian to yeild a valid (4dof) unobservable nullspace. -As such, the system observability properities can be preserved. While not utilizing the current state estimation for linearization may go against intuition, FEJ has proven to enhance the accuracy and consistency of VINS in many cases. -FEJ's use of the first state estimates may introduce of "unmodeled" error into the filter, which is a potential issue to keep in mind. -This problem has been discussed and address in our follow up work FEJ2 @cite Chen2022ICRA and OC @cite Huang2010IJRR @cite Hesch2013TRO @cite Hesch2014IJRR. For interested readers, we recommend exploring these follow-up studies. - */ + +where \f$ \mathbf{h}(\cdot) \f$ is the nonlinear measurement function and \f$ \bar{\mathbf{H}}_{k} \f$ is the First-Estimate Jacobian which only consist the first state estimates \f$ \bar{\mathbf{x}}_k \f$. +It is not difficult to confirm that the observability matrix \f$ \mathcal{O} \f$ with the FEJ state transition matrix and measurement Jacobians yields the correct 4dof unobservable nullspace. +As such, the system observability properties can be preserved. + +While not utilizing the current (best) state estimates for linearization does introduce linearization errors, it has been shown that the inconsistencies far outweigh this. +Follow up works which have also tried to address this problem include FEJ2 @cite Chen2022ICRA and OC @cite Huang2010IJRR @cite Hesch2013TRO @cite Hesch2014IJRR. +Both of these works try to handle the problem of ensuring correct observability properties while compensating or leveraging the best estimates. + + + +*/ From dc09c66d4f996f2c1146bd33dd788e0bee50082b Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 11 May 2023 17:35:57 -0400 Subject: [PATCH 49/60] pr fixes --- ReadMe.md | 2 +- config/rpng_plane/estimator_config.yaml | 4 +--- config/rs_d455/estimator_config.yaml | 4 +--- config/rs_t265/estimator_config.yaml | 4 +--- ov_core/src/utils/opencv_yaml_parse.h | 3 ++- ov_core/src/utils/sensor_data.h | 2 -- ov_msckf/launch/serial.launch | 2 +- ov_msckf/scripts/run_ros_eth.sh | 14 +++++++------- ov_msckf/scripts/run_ros_kaist.sh | 6 +++--- ov_msckf/scripts/run_ros_tumvi.sh | 4 ++-- ov_msckf/scripts/run_sim_calib.sh | 4 ++-- ov_msckf/scripts/run_sim_cams.sh | 4 ++-- ov_msckf/scripts/run_sim_featrep.sh | 4 ++-- 13 files changed, 25 insertions(+), 32 deletions(-) diff --git a/ReadMe.md b/ReadMe.md index 13caf9ee7..1448a49c1 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -21,7 +21,7 @@ details on what the system supports. ## News / Events - +* **May 11, 2023** - Inertial intrinsic support released as part of v2.7 along with a few bug fixes and improvements to stereo KLT tracking. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.7) for details. * **April 15, 2023** - Minor update to v2.6.3 to support incremental feature triangulation of active features for downstream applications, faster zero-velocity update, small bug fixes, some example realsense configurations, and cached fast state prediction. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.6.3) for details. * **April 3, 2023** - We have released a monocular plane-aided VINS, termed [ov_plane](https://github.com/rpng/ov_plane), which leverages the OpenVINS project. Both now support the released [Indoor AR Table](https://github.com/rpng/ar_table_dataset) dataset. * **July 14, 2022** - Improved feature extraction logic for >100hz tracking, some bug fixes and updated scripts. See v2.6.1 [PR#259](https://github.com/rpng/open_vins/pull/259) and v2.6.2 [PR#264](https://github.com/rpng/open_vins/pull/264). diff --git a/config/rpng_plane/estimator_config.yaml b/config/rpng_plane/estimator_config.yaml index b1ee58b27..04c0774e4 100644 --- a/config/rpng_plane/estimator_config.yaml +++ b/config/rpng_plane/estimator_config.yaml @@ -3,9 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) diff --git a/config/rs_d455/estimator_config.yaml b/config/rs_d455/estimator_config.yaml index 64d705ffd..a1e78fc2f 100644 --- a/config/rs_d455/estimator_config.yaml +++ b/config/rs_d455/estimator_config.yaml @@ -3,9 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) diff --git a/config/rs_t265/estimator_config.yaml b/config/rs_t265/estimator_config.yaml index 6cfad3214..4604ede56 100644 --- a/config/rs_t265/estimator_config.yaml +++ b/config/rs_t265/estimator_config.yaml @@ -3,9 +3,7 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index 568f7011b..3ee981483 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -43,7 +43,8 @@ namespace ov_core { * @brief Helper class to do OpenCV yaml parsing from both file and ROS. * * The logic is as follows: - * - Given a path to the main config file we will load it into our [cv::FileStorage](https://docs.opencv.org/4.x/da/d56/classcv_1_1FileStorage.html) object. + * - Given a path to the main config file we will load it into our + * [cv::FileStorage](https://docs.opencv.org/4.x/da/d56/classcv_1_1FileStorage.html) object. * - From there the user can request for different parameters of different types from the config. * - If we have ROS, then we will also check to see if the user has overridden any config files via ROS. * - The ROS parameters always take priority over the ones in our config. diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h index 17d01f345..98811f638 100644 --- a/ov_core/src/utils/sensor_data.h +++ b/ov_core/src/utils/sensor_data.h @@ -26,8 +26,6 @@ #include #include -#include "utils/quat_ops.h" - namespace ov_core { /** diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 7ce9ed8f9..0ba496e34 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -11,7 +11,7 @@ - + diff --git a/ov_msckf/scripts/run_ros_eth.sh b/ov_msckf/scripts/run_ros_eth.sh index 91cd27878..9aa43dc31 100755 --- a/ov_msckf/scripts/run_ros_eth.sh +++ b/ov_msckf/scripts/run_ros_eth.sh @@ -23,11 +23,11 @@ bagnames=( "V2_01_easy" "V2_02_medium" "V2_03_difficult" - "MH_01_easy" - "MH_02_easy" - "MH_03_medium" - "MH_04_difficult" - "MH_05_difficult" +# "MH_01_easy" +# "MH_02_easy" +# "MH_03_medium" +# "MH_04_difficult" +# "MH_05_difficult" ) # how far we should start into the dataset @@ -49,8 +49,8 @@ bagstarttimes=( # location to save log files into save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_euroc/algorithms" save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_euroc/timings" -bag_path="/home/patrick/datasets/euroc_mav/" -ov_ver="2.6.3" +bag_path="/media/patrick/Windows/datasets/euroc_mav/" +ov_ver="2.7" #============================================================= diff --git a/ov_msckf/scripts/run_ros_kaist.sh b/ov_msckf/scripts/run_ros_kaist.sh index bb92f00a5..bccace9a1 100755 --- a/ov_msckf/scripts/run_ros_kaist.sh +++ b/ov_msckf/scripts/run_ros_kaist.sh @@ -18,7 +18,7 @@ modes=( # dataset locations bagnames=( "urban28" - "urban32" +# "urban32" # "urban34" # too strong of sun... "urban38" "urban39" @@ -37,8 +37,8 @@ bagstarttimes=( # location to save log files into save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_kaist/algorithms" save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_kaist/timings" -bag_path="/home/patrick/datasets/euroc_mav/" -ov_ver="2.6.3" +bag_path="/media/patrick/Windows/datasets/kaist/" +ov_ver="2.7" #============================================================= diff --git a/ov_msckf/scripts/run_ros_tumvi.sh b/ov_msckf/scripts/run_ros_tumvi.sh index 76e15bf5f..15aace19f 100755 --- a/ov_msckf/scripts/run_ros_tumvi.sh +++ b/ov_msckf/scripts/run_ros_tumvi.sh @@ -39,8 +39,8 @@ bagstarttimes=( # location to save log files into save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_tumvi/algorithms" save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_tumvi/timings" -bag_path="/home/patrick/datasets/tum_vi/" -ov_ver="2.6.3" +bag_path="/media/patrick/Windows/datasets/tum_vi/" +ov_ver="2.7" #============================================================= diff --git a/ov_msckf/scripts/run_sim_calib.sh b/ov_msckf/scripts/run_sim_calib.sh index 40a7d72ce..c1837ea66 100755 --- a/ov_msckf/scripts/run_sim_calib.sh +++ b/ov_msckf/scripts/run_sim_calib.sh @@ -10,10 +10,10 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash # datasets datasets=( - "udel_gore" +# "udel_gore" # "udel_arl" # "udel_gore_zupt" -# "tum_corridor1_512_16_okvis" + "tum_corridor1_512_16_okvis" ) # If we want to calibrate parameters diff --git a/ov_msckf/scripts/run_sim_cams.sh b/ov_msckf/scripts/run_sim_cams.sh index 132d02dc4..12f18e753 100755 --- a/ov_msckf/scripts/run_sim_cams.sh +++ b/ov_msckf/scripts/run_sim_cams.sh @@ -10,10 +10,10 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash # datasets datasets=( - "udel_gore" +# "udel_gore" # "udel_arl" # "udel_gore_zupt" -# "tum_corridor1_512_16_okvis" + "tum_corridor1_512_16_okvis" ) # what modes to use diff --git a/ov_msckf/scripts/run_sim_featrep.sh b/ov_msckf/scripts/run_sim_featrep.sh index b97c81533..3edaa5c0d 100755 --- a/ov_msckf/scripts/run_sim_featrep.sh +++ b/ov_msckf/scripts/run_sim_featrep.sh @@ -10,10 +10,10 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash # datasets datasets=( - "udel_gore" +# "udel_gore" # "udel_arl" # "udel_gore_zupt" -# "tum_corridor1_512_16_okvis" + "tum_corridor1_512_16_okvis" ) # estimator configurations From 40342818071b2f2c9bb76f4dc800521f374be533 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sat, 13 May 2023 17:27:14 -0400 Subject: [PATCH 50/60] ensure to free memory, reduce strictness of num feature percent to do init --- ov_init/src/dynamic/DynamicInitializer.cpp | 27 +++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/ov_init/src/dynamic/DynamicInitializer.cpp b/ov_init/src/dynamic/DynamicInitializer.cpp index dc8192fa8..e69405dcc 100644 --- a/ov_init/src/dynamic/DynamicInitializer.cpp +++ b/ov_init/src/dynamic/DynamicInitializer.cpp @@ -69,7 +69,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian have_old_imu_readings = true; it_imu = imu_data->erase(it_imu); } - if (_db->get_internal_data().size() < 0.95 * params.init_max_features) { + if (_db->get_internal_data().size() < 0.75 * params.init_max_features) { PRINT_WARNING(RED "[init-d]: only %zu valid features of required (%.0f thresh)!!\n" RESET, _db->get_internal_data().size(), 0.95 * params.init_max_features); return false; @@ -602,6 +602,28 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian std::map map_calib_cam; std::vector ceres_vars_calib_cam_intrinsics; + // Helper lambda that will free any memory we have allocated + auto free_state_memory = [&]() { + for (auto const &ptr : ceres_vars_ori) + delete[] ptr; + for (auto const &ptr : ceres_vars_pos) + delete[] ptr; + for (auto const &ptr : ceres_vars_vel) + delete[] ptr; + for (auto const &ptr : ceres_vars_bias_g) + delete[] ptr; + for (auto const &ptr : ceres_vars_bias_a) + delete[] ptr; + for (auto const &ptr : ceres_vars_feat) + delete[] ptr; + for (auto const &ptr : ceres_vars_calib_cam2imu_ori) + delete[] ptr; + for (auto const &ptr : ceres_vars_calib_cam2imu_pos) + delete[] ptr; + for (auto const &ptr : ceres_vars_calib_cam_intrinsics) + delete[] ptr; + }; + // Set the optimization settings // NOTE: We use dense schur since after eliminating features we have a dense problem // NOTE: http://ceres-solver.org/solving_faqs.html#solving @@ -887,6 +909,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian timestamp = newest_cam_time; if (params.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE) { PRINT_WARNING(YELLOW "[init-d]: opt failed: %s!\n" RESET, summary.message.c_str()); + free_state_memory(); return false; } PRINT_DEBUG("[init-d]: %s\n", summary.message.c_str()); @@ -997,6 +1020,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian bool success = problem_cov.Compute(covariance_blocks, &problem); if (!success) { PRINT_WARNING(YELLOW "[init-d]: covariance recovery failed...\n" RESET); + free_state_memory(); return false; } @@ -1086,5 +1110,6 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian PRINT_DEBUG("[TIME]: %.4f sec for ceres opt\n", (rT6 - rT5).total_microseconds() * 1e-6); PRINT_DEBUG("[TIME]: %.4f sec for ceres covariance\n", (rT7 - rT6).total_microseconds() * 1e-6); PRINT_DEBUG("[TIME]: %.4f sec total for initialization\n", (rT7 - rT1).total_microseconds() * 1e-6); + free_state_memory(); return true; } From f10ce282facde6261691e43183c3e828ecce3281 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sat, 13 May 2023 17:27:43 -0400 Subject: [PATCH 51/60] we have only init'ed once we have updated (fix messy viz in beginning) --- ov_msckf/src/core/VioManager.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 56f85a1e2..3fd708901 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -96,7 +96,7 @@ class VioManager { void initialize_with_gt(Eigen::Matrix imustate); /// If we are initialized or not - bool initialized() { return is_initialized_vio; } + bool initialized() { return is_initialized_vio && timelastupdate != -1; } /// Timestamp that the system was initialized at double initialized_time() { return startup_time; } From 4492d090b701f73a7e8799de9e8372f7da14f766 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sun, 14 May 2023 19:55:31 -0400 Subject: [PATCH 52/60] only dynamic init if we have good disparity --- ov_init/src/init/InertialInitializer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp index 04e51071c..1ee82175c 100644 --- a/ov_init/src/init/InertialInitializer.cpp +++ b/ov_init/src/init/InertialInitializer.cpp @@ -132,7 +132,7 @@ bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covaria if (((has_jerk && wait_for_jerk) || (is_still && !wait_for_jerk)) && params.init_imu_thresh > 0.0) { PRINT_DEBUG(GREEN "[init]: USING STATIC INITIALIZER METHOD!\n" RESET); return init_static->initialize(timestamp, covariance, order, t_imu, wait_for_jerk); - } else if (params.init_dyn_use) { + } else if (params.init_dyn_use && !is_still) { PRINT_DEBUG(GREEN "[init]: USING DYNAMIC INITIALIZER METHOD!\n" RESET); std::map> _clones_IMU; std::unordered_map> _features_SLAM; From 78987b1bf52a52e6c79fcbef20f84473e6cbdb26 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sun, 14 May 2023 19:55:44 -0400 Subject: [PATCH 53/60] update configs a bit --- config/euroc_mav/estimator_config.yaml | 2 +- config/kaist/estimator_config.yaml | 2 +- config/kaist_vio/estimator_config.yaml | 2 +- config/rpng_aruco/estimator_config.yaml | 2 +- config/rpng_ironsides/estimator_config.yaml | 2 +- config/rpng_plane/estimator_config.yaml | 2 +- config/rs_d455/estimator_config.yaml | 2 +- config/rs_t265/estimator_config.yaml | 2 +- config/tum_vi/estimator_config.yaml | 2 +- config/uzhfpv_indoor/estimator_config.yaml | 2 +- config/uzhfpv_indoor_45/estimator_config.yaml | 2 +- config/uzhfpv_outdoor/estimator_config.yaml | 2 +- .../uzhfpv_outdoor_45/estimator_config.yaml | 2 +- ov_msckf/scripts/run_ros_eth.sh | 20 +++++++++---------- ov_msckf/scripts/run_ros_uzhfpv.sh | 4 ++-- 15 files changed, 25 insertions(+), 25 deletions(-) diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index 094958363..560750fc0 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 75 # how many features to track during initialization (saves on computation) +init_max_features: 50 # how many features to track during initialization (saves on computation) init_dyn_use: false # if dynamic initialization should be used init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) diff --git a/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml index 1a2b986e3..c72fac448 100644 --- a/config/kaist/estimator_config.yaml +++ b/config/kaist/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 75 +init_max_features: 50 init_dyn_use: true init_dyn_mle_opt_calib: false diff --git a/config/kaist_vio/estimator_config.yaml b/config/kaist_vio/estimator_config.yaml index 7b96ad6d9..999a95e4c 100644 --- a/config/kaist_vio/estimator_config.yaml +++ b/config/kaist_vio/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 0.60 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 5.0 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 75 # how many features to track during initialization (saves on computation) +init_max_features: 50 # how many features to track during initialization (saves on computation) init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml index 9a9be23d3..02e8db9aa 100644 --- a/config/rpng_aruco/estimator_config.yaml +++ b/config/rpng_aruco/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: true init_window_time: 2.0 init_imu_thresh: 1.2 init_max_disparity: 2.0 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml index 039cfd4b1..27dde860d 100644 --- a/config/rpng_ironsides/estimator_config.yaml +++ b/config/rpng_ironsides/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/rpng_plane/estimator_config.yaml b/config/rpng_plane/estimator_config.yaml index 04c0774e4..e2f779c13 100644 --- a/config/rpng_plane/estimator_config.yaml +++ b/config/rpng_plane/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: true init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 0.5 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 4.0 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 75 # how many features to track during initialization (saves on computation) +init_max_features: 50 # how many features to track during initialization (saves on computation) init_dyn_use: false # if dynamic initialization should be used init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) diff --git a/config/rs_d455/estimator_config.yaml b/config/rs_d455/estimator_config.yaml index a1e78fc2f..226295345 100644 --- a/config/rs_d455/estimator_config.yaml +++ b/config/rs_d455/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 75 # how many features to track during initialization (saves on computation) +init_max_features: 50 # how many features to track during initialization (saves on computation) init_dyn_use: false # if dynamic initialization should be used init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) diff --git a/config/rs_t265/estimator_config.yaml b/config/rs_t265/estimator_config.yaml index 4604ede56..21b0b8623 100644 --- a/config/rs_t265/estimator_config.yaml +++ b/config/rs_t265/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 75 # how many features to track during initialization (saves on computation) +init_max_features: 50 # how many features to track during initialization (saves on computation) init_dyn_use: false # if dynamic initialization should be used init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml index 20f0db1be..708cb3e53 100644 --- a/config/tum_vi/estimator_config.yaml +++ b/config/tum_vi/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: true init_window_time: 1.5 # make 2sec if using dynamic... init_imu_thresh: 0.45 # room1-5:0.45, room6:0.25 init_max_disparity: 15.0 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml index 5f3ab1223..f1859a319 100644 --- a/config/uzhfpv_indoor/estimator_config.yaml +++ b/config/uzhfpv_indoor/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 init_imu_thresh: 0.30 init_max_disparity: 2.0 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml index 5f3ab1223..f1859a319 100644 --- a/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/config/uzhfpv_indoor_45/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 init_imu_thresh: 0.30 init_max_disparity: 2.0 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/uzhfpv_outdoor/estimator_config.yaml b/config/uzhfpv_outdoor/estimator_config.yaml index 79dbddfef..6f548165d 100644 --- a/config/uzhfpv_outdoor/estimator_config.yaml +++ b/config/uzhfpv_outdoor/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 init_imu_thresh: 0.30 init_max_disparity: 2.0 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/config/uzhfpv_outdoor_45/estimator_config.yaml b/config/uzhfpv_outdoor_45/estimator_config.yaml index a1b000412..d0abb79cf 100644 --- a/config/uzhfpv_outdoor_45/estimator_config.yaml +++ b/config/uzhfpv_outdoor_45/estimator_config.yaml @@ -40,7 +40,7 @@ zupt_only_at_beginning: false init_window_time: 2.0 init_imu_thresh: 0.30 init_max_disparity: 2.0 -init_max_features: 75 +init_max_features: 50 init_dyn_use: false init_dyn_mle_opt_calib: false diff --git a/ov_msckf/scripts/run_ros_eth.sh b/ov_msckf/scripts/run_ros_eth.sh index 9aa43dc31..7c0e5c150 100755 --- a/ov_msckf/scripts/run_ros_eth.sh +++ b/ov_msckf/scripts/run_ros_eth.sh @@ -23,11 +23,11 @@ bagnames=( "V2_01_easy" "V2_02_medium" "V2_03_difficult" -# "MH_01_easy" -# "MH_02_easy" -# "MH_03_medium" -# "MH_04_difficult" -# "MH_05_difficult" + "MH_01_easy" + "MH_02_easy" + "MH_03_medium" + "MH_04_difficult" + "MH_05_difficult" ) # how far we should start into the dataset @@ -39,11 +39,11 @@ bagstarttimes=( "0" "0" "0" - "40" # 40 - "35" # 35 - "5" # 10 - "10" # 17 - "5" # 18 + "40" + "35" + "5" + "10" + "5" # stereo can fail if starts while still due to bad left-right KLT.... ) # location to save log files into diff --git a/ov_msckf/scripts/run_ros_uzhfpv.sh b/ov_msckf/scripts/run_ros_uzhfpv.sh index 1b08f4abb..392a547cb 100755 --- a/ov_msckf/scripts/run_ros_uzhfpv.sh +++ b/ov_msckf/scripts/run_ros_uzhfpv.sh @@ -82,8 +82,8 @@ bagstarttimes=( # location to save log files into save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_uzhfpv/algorithms" save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_uzhfpv/timings" -bag_path="/home/patrick/datasets/euroc_mav/" -ov_ver="2.6.3" +bag_path="/media/patrick/RPNG FLASH 3/" +ov_ver="2.7" #============================================================= From 022b63370c04ec8f173a561f20cceda270a22d9b Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 18 May 2023 10:49:15 -0400 Subject: [PATCH 54/60] update package version --- ov_core/package.xml | 2 +- ov_data/package.xml | 2 +- ov_eval/package.xml | 2 +- ov_init/package.xml | 2 +- ov_msckf/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ov_core/package.xml b/ov_core/package.xml index 76192b66f..f4a0e7d8a 100644 --- a/ov_core/package.xml +++ b/ov_core/package.xml @@ -3,7 +3,7 @@ ov_core - 2.6.3 + 2.7.0 Core algorithms for visual-inertial navigation algorithms. diff --git a/ov_data/package.xml b/ov_data/package.xml index 0ec169cd4..dc249cab5 100644 --- a/ov_data/package.xml +++ b/ov_data/package.xml @@ -3,7 +3,7 @@ ov_data - 2.6.3 + 2.7.0 Data for the OpenVINS project, mostly just groundtruth files... diff --git a/ov_eval/package.xml b/ov_eval/package.xml index 4155ea3bc..f63af370a 100644 --- a/ov_eval/package.xml +++ b/ov_eval/package.xml @@ -3,7 +3,7 @@ ov_eval - 2.6.3 + 2.7.0 Evaluation methods and scripts for visual-inertial odometry systems. diff --git a/ov_init/package.xml b/ov_init/package.xml index ad8ccb483..8a39028cb 100644 --- a/ov_init/package.xml +++ b/ov_init/package.xml @@ -3,7 +3,7 @@ ov_init - 2.6.3 + 2.7.0 Initialization package which handles static and dynamic initialization. diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index 09d0412b7..3386aba83 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -3,7 +3,7 @@ ov_msckf - 2.6.3 + 2.7.0 Implementation of a type-based error-state Kalman filter. From 2b506eeedd0b158c014641b9240a62ae80f6d7a0 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 18 May 2023 10:55:18 -0400 Subject: [PATCH 55/60] label how to get imu intrinsics from kalibr result file --- config/euroc_mav/kalibr_imu_chain.yaml | 10 +++++++++- config/kaist/kalibr_imu_chain.yaml | 10 +++++++++- config/kaist_vio/kalibr_imu_chain.yaml | 10 +++++++++- config/rpng_aruco/kalibr_imu_chain.yaml | 10 +++++++++- config/rpng_ironsides/kalibr_imu_chain.yaml | 10 +++++++++- config/rpng_plane/kalibr_imu_chain.yaml | 10 +++++++++- config/rpng_sim/kalibr_imu_chain.yaml | 10 +++++++++- config/rs_d455/kalibr_imu_chain.yaml | 10 +++++++++- config/rs_t265/kalibr_imu_chain.yaml | 17 ++++++++++------- config/tum_vi/kalibr_imu_chain.yaml | 10 +++++++++- config/uzhfpv_indoor/kalibr_imu_chain.yaml | 10 +++++++++- config/uzhfpv_indoor_45/kalibr_imu_chain.yaml | 10 +++++++++- config/uzhfpv_outdoor/kalibr_imu_chain.yaml | 10 +++++++++- config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml | 10 +++++++++- 14 files changed, 127 insertions(+), 20 deletions(-) diff --git a/config/euroc_mav/kalibr_imu_chain.yaml b/config/euroc_mav/kalibr_imu_chain.yaml index b7e0e6003..c85f0f677 100644 --- a/config/euroc_mav/kalibr_imu_chain.yaml +++ b/config/euroc_mav/kalibr_imu_chain.yaml @@ -13,7 +13,15 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/kaist/kalibr_imu_chain.yaml b/config/kaist/kalibr_imu_chain.yaml index 83cdb1bb6..8066a5634 100644 --- a/config/kaist/kalibr_imu_chain.yaml +++ b/config/kaist/kalibr_imu_chain.yaml @@ -21,7 +21,15 @@ imu0: rostopic: /imu/data_raw time_offset: 0.0 update_rate: 500.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/kaist_vio/kalibr_imu_chain.yaml b/config/kaist_vio/kalibr_imu_chain.yaml index 3ae087a09..9b57b634f 100644 --- a/config/kaist_vio/kalibr_imu_chain.yaml +++ b/config/kaist_vio/kalibr_imu_chain.yaml @@ -17,7 +17,15 @@ imu0: rostopic: /mavros/imu/data time_offset: 0.0 update_rate: 100.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rpng_aruco/kalibr_imu_chain.yaml b/config/rpng_aruco/kalibr_imu_chain.yaml index 8a4d0bbe8..401ddbfdc 100644 --- a/config/rpng_aruco/kalibr_imu_chain.yaml +++ b/config/rpng_aruco/kalibr_imu_chain.yaml @@ -13,7 +13,15 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rpng_ironsides/kalibr_imu_chain.yaml b/config/rpng_ironsides/kalibr_imu_chain.yaml index 7168253dd..d5144b3a9 100644 --- a/config/rpng_ironsides/kalibr_imu_chain.yaml +++ b/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -13,7 +13,15 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rpng_plane/kalibr_imu_chain.yaml b/config/rpng_plane/kalibr_imu_chain.yaml index 38dab7bd4..e078b037e 100644 --- a/config/rpng_plane/kalibr_imu_chain.yaml +++ b/config/rpng_plane/kalibr_imu_chain.yaml @@ -13,7 +13,15 @@ imu0: rostopic: /d455/imu time_offset: 0.0 update_rate: 400 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml index 8a4d0bbe8..401ddbfdc 100644 --- a/config/rpng_sim/kalibr_imu_chain.yaml +++ b/config/rpng_sim/kalibr_imu_chain.yaml @@ -13,7 +13,15 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rs_d455/kalibr_imu_chain.yaml b/config/rs_d455/kalibr_imu_chain.yaml index cdcf59533..7b3c7b06b 100644 --- a/config/rs_d455/kalibr_imu_chain.yaml +++ b/config/rs_d455/kalibr_imu_chain.yaml @@ -22,7 +22,15 @@ imu0: rostopic: /d455/imu time_offset: 0.0 update_rate: 400 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/rs_t265/kalibr_imu_chain.yaml b/config/rs_t265/kalibr_imu_chain.yaml index 876b54833..2159965f9 100644 --- a/config/rs_t265/kalibr_imu_chain.yaml +++ b/config/rs_t265/kalibr_imu_chain.yaml @@ -6,12 +6,7 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - # Values from allan plots - # https://github.com/rpng/ar_table_dataset - #accelerometer_noise_density: 0.001464164197530967 - #accelerometer_random_walk: 6.696534319519138e-05 - #gyroscope_noise_density: 0.00010833365085022152 - #gyroscope_random_walk: 1.1687657234840692e-06 + # Values from allan plots of ~20hour dataset # Inflated values (to account for unmodelled effects) # - white noise multiplied by 5 # - bias random walk multiplied by 10 @@ -22,7 +17,15 @@ imu0: rostopic: /t265/imu time_offset: 0.0 update_rate: 200 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/tum_vi/kalibr_imu_chain.yaml b/config/tum_vi/kalibr_imu_chain.yaml index 35a8e844b..9c474d1a8 100644 --- a/config/tum_vi/kalibr_imu_chain.yaml +++ b/config/tum_vi/kalibr_imu_chain.yaml @@ -13,7 +13,15 @@ imu0: rostopic: /imu0 time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/config/uzhfpv_indoor/kalibr_imu_chain.yaml index d02d12239..dec36b504 100644 --- a/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -17,7 +17,15 @@ imu0: rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml index d02d12239..dec36b504 100644 --- a/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml +++ b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -17,7 +17,15 @@ imu0: rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/uzhfpv_outdoor/kalibr_imu_chain.yaml b/config/uzhfpv_outdoor/kalibr_imu_chain.yaml index d02d12239..dec36b504 100644 --- a/config/uzhfpv_outdoor/kalibr_imu_chain.yaml +++ b/config/uzhfpv_outdoor/kalibr_imu_chain.yaml @@ -17,7 +17,15 @@ imu0: rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] diff --git a/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml b/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml index d02d12239..dec36b504 100644 --- a/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml +++ b/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml @@ -17,7 +17,15 @@ imu0: rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 - model: "kalibr" # calibrated (same as kalibr), kalibr, rpng + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: Tw: - [ 1.0, 0.0, 0.0 ] - [ 0.0, 1.0, 0.0 ] From 7e4dd43aa9076af843dedae80c0f23e0be945c91 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 22 May 2023 15:55:19 -0400 Subject: [PATCH 56/60] 2d ate in comparison eval, report bad trajectory distance ratios to help debugging --- ov_eval/src/calc/ResultTrajectory.cpp | 7 ++++ ov_eval/src/error_comparison.cpp | 54 +++++++++++++++++++++++++-- 2 files changed, 58 insertions(+), 3 deletions(-) diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp index 33a13b695..6a5abfe39 100644 --- a/ov_eval/src/calc/ResultTrajectory.cpp +++ b/ov_eval/src/calc/ResultTrajectory.cpp @@ -44,6 +44,13 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); std::exit(EXIT_FAILURE); } + double len_gt = ov_eval::Loader::get_total_length(gt_poses); + double len_est = ov_eval::Loader::get_total_length(est_poses); + double ratio = len_est / len_gt; + if (ratio > 1.1 || ratio < 0.9) { + PRINT_WARNING(YELLOW "[TRAJ]: Trajectory is a bad ratio of %.2f length (est %.2f, gt %.2f)\n", ratio, len_est, len_gt); + PRINT_WARNING(YELLOW "[TRAJ]: %s\n", path_est.c_str()); + } // Perform alignment of the trajectories Eigen::Matrix3d R_ESTtoGT, R_GTtoEST; diff --git a/ov_eval/src/error_comparison.cpp b/ov_eval/src/error_comparison.cpp index 89d26e860..9cda7627b 100644 --- a/ov_eval/src/error_comparison.cpp +++ b/ov_eval/src/error_comparison.cpp @@ -91,12 +91,14 @@ int main(int argc, char **argv) { // ATE summery information std::map>> algo_ate; + std::map>> algo_ate_2d; for (const auto &p : path_algorithms) { std::vector> temp; for (size_t i = 0; i < path_groundtruths.size(); i++) { temp.push_back({ov_eval::Statistics(), ov_eval::Statistics()}); } algo_ate.insert({p.filename().string(), temp}); + algo_ate_2d.insert({p.filename().string(), temp}); } // Relative pose error segment lengths @@ -104,7 +106,7 @@ int main(int argc, char **argv) { // std::vector segments = {7.0, 14.0, 21.0, 28.0, 35.0}; // std::vector segments = {10.0, 25.0, 50.0, 75.0, 120.0}; // std::vector segments = {5.0, 15.0, 30.0, 45.0, 60.0}; - // std::vector segments = {40.0, 60.0, 80.0, 100.0, 120.0}; + // std::vector segments = {40.0, 80.0, 120.0, 160.0, 200.0, 240.0}; // The overall RPE error calculation for each algorithm type std::map>> algo_rpe; @@ -150,8 +152,8 @@ int main(int argc, char **argv) { path_groundtruths.at(j).stem().c_str()); // Errors for this specific dataset (i.e. our averages over the total runs) - ov_eval::Statistics ate_dataset_ori; - ov_eval::Statistics ate_dataset_pos; + ov_eval::Statistics ate_dataset_ori, ate_dataset_pos; + ov_eval::Statistics ate_2d_dataset_ori, ate_2d_dataset_pos; std::map> rpe_dataset; for (const auto &len : segments) { rpe_dataset.insert({len, {ov_eval::Statistics(), ov_eval::Statistics()}}); @@ -181,6 +183,12 @@ int main(int argc, char **argv) { ate_dataset_ori.values.push_back(error_ori.rmse); ate_dataset_pos.values.push_back(error_pos.rmse); + // Calculate ATE 2D error for this dataset + ov_eval::Statistics error_ori_2d, error_pos_2d; + traj.calculate_ate_2d(error_ori_2d, error_pos_2d); + ate_2d_dataset_ori.values.push_back(error_ori_2d.rmse); + ate_2d_dataset_pos.values.push_back(error_pos_2d.rmse); + // Calculate RPE error for this dataset std::map> error_rpe; traj.calculate_rpe(segments, error_rpe); @@ -199,12 +207,17 @@ int main(int argc, char **argv) { // Compute our mean ATE score ate_dataset_ori.calculate(); ate_dataset_pos.calculate(); + ate_2d_dataset_ori.calculate(); + ate_2d_dataset_pos.calculate(); // Print stats for this specific dataset std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : ""; PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean, (int)ate_dataset_pos.values.size()); PRINT_DEBUG("\tATE: std_ori = %.3f | std_pos = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std); + PRINT_DEBUG("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean, + (int)ate_2d_dataset_ori.values.size()); + PRINT_DEBUG("\tATE 2D: std_ori = %.5f | std_pos = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std); for (auto &seg : rpe_dataset) { seg.second.first.calculate(); seg.second.second.calculate(); @@ -219,6 +232,8 @@ int main(int argc, char **argv) { std::string algo = path_algorithms.at(i).filename().string(); algo_ate.at(algo).at(j).first = ate_dataset_ori; algo_ate.at(algo).at(j).second = ate_dataset_pos; + algo_ate_2d.at(algo).at(j).first = ate_2d_dataset_ori; + algo_ate_2d.at(algo).at(j).second = ate_2d_dataset_pos; // Update the global RPE error stats for (const auto &elm : rpe_dataset) { @@ -272,6 +287,39 @@ int main(int argc, char **argv) { } PRINT_INFO("============================================\n"); + // Finally print the ATE for all the runs + PRINT_INFO("============================================\n"); + PRINT_INFO("ATE 2D LATEX TABLE\n"); + PRINT_INFO("============================================\n"); + for (size_t i = 0; i < path_groundtruths.size(); i++) { + std::string gtname = path_groundtruths.at(i).stem().string(); + boost::replace_all(gtname, "_", "\\_"); + PRINT_INFO(" & \\textbf{%s}", gtname.c_str()); + } + PRINT_INFO(" & \\textbf{Average} \\\\\\hline\n"); + for (auto &algo : algo_ate_2d) { + std::string algoname = algo.first; + boost::replace_all(algoname, "_", "\\_"); + PRINT_INFO(algoname.c_str()); + double sum_ori = 0.0; + double sum_pos = 0.0; + int sum_ct = 0; + for (auto &seg : algo.second) { + if (seg.first.values.empty() || seg.second.values.empty()) { + PRINT_INFO(" & - / -"); + } else { + seg.first.calculate(); + seg.second.calculate(); + PRINT_INFO(" & %.3f / %.3f", seg.first.mean, seg.second.mean); + sum_ori += seg.first.mean; + sum_pos += seg.second.mean; + sum_ct++; + } + } + PRINT_INFO(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct); + } + PRINT_INFO("============================================\n"); + // Finally print the RPE for all the runs PRINT_INFO("============================================\n"); PRINT_INFO("RPE LATEX TABLE\n"); From 456907c45ddeb71a4e0ad2e5693d8024987a47da Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 23 May 2023 14:29:42 -0400 Subject: [PATCH 57/60] compute ratio before associating --- ov_eval/src/calc/ResultTrajectory.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp index 6a5abfe39..851d346ee 100644 --- a/ov_eval/src/calc/ResultTrajectory.cpp +++ b/ov_eval/src/calc/ResultTrajectory.cpp @@ -34,6 +34,13 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st // std::string base_filename2 = path_gt.substr(path_gt.find_last_of("/\\") + 1); // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str()); // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str()); + double len_gt = ov_eval::Loader::get_total_length(gt_poses); + double len_est = ov_eval::Loader::get_total_length(est_poses); + double ratio = len_est / len_gt; + if (ratio > 1.1 || ratio < 0.9) { + PRINT_WARNING(YELLOW "[TRAJ]: Trajectory is a bad ratio of %.2f length (est %.2f, gt %.2f)\n", ratio, len_est, len_gt); + PRINT_WARNING(YELLOW "[TRAJ]: %s\n", path_est.c_str()); + } // Intersect timestamps AlignUtils::perform_association(0, 0.02, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori, gt_covpos); @@ -44,13 +51,6 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); std::exit(EXIT_FAILURE); } - double len_gt = ov_eval::Loader::get_total_length(gt_poses); - double len_est = ov_eval::Loader::get_total_length(est_poses); - double ratio = len_est / len_gt; - if (ratio > 1.1 || ratio < 0.9) { - PRINT_WARNING(YELLOW "[TRAJ]: Trajectory is a bad ratio of %.2f length (est %.2f, gt %.2f)\n", ratio, len_est, len_gt); - PRINT_WARNING(YELLOW "[TRAJ]: %s\n", path_est.c_str()); - } // Perform alignment of the trajectories Eigen::Matrix3d R_ESTtoGT, R_GTtoEST; From 54e483e8b89472ecebe89cd0809ea544dbfc1642 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 8 Jun 2023 14:10:46 -0400 Subject: [PATCH 58/60] fix #340 feat init jacob error --- docs/update-featinit.dox | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/update-featinit.dox b/docs/update-featinit.dox index 93232d125..e717496df 100644 --- a/docs/update-featinit.dox +++ b/docs/update-featinit.dox @@ -264,8 +264,8 @@ Therefore, we can have the least-squares formulated and Jacobians: \frac{\partial \mathbf{h}(u_A, v_A, \rho_A)}{\partial {h}(u_A, v_A, \rho_A)} & = \begin{bmatrix} - 1/h(\cdots)(1) & 0 & -h(\cdots)(1)/h(\cdots)(3)^2 \\ - 0 & 1/h(\cdots)(2) & -h(\cdots)(2)/h(\cdots)(3)^2 + 1/h(\cdots)(3) & 0 & -h(\cdots)(1)/h(\cdots)(3)^2 \\ + 0 & 1/h(\cdots)(3) & -h(\cdots)(2)/h(\cdots)(3)^2 \end{bmatrix} \\ \frac{\partial {h}(u_A, v_A, \rho_A)}{\partial [u_A, v_A, \rho_A]} & = From 2d5ed9ac5d3e911b1c11e1703525715804b9968c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 8 Jun 2023 17:00:53 -0400 Subject: [PATCH 59/60] fix #296 more comments and switch to sparse qr recovery for speed --- ov_init/src/dynamic/DynamicInitializer.cpp | 50 +++++++++------------- ov_init/src/test_dynamic_init.cpp | 4 +- ov_init/src/test_dynamic_mle.cpp | 4 +- 3 files changed, 25 insertions(+), 33 deletions(-) diff --git a/ov_init/src/dynamic/DynamicInitializer.cpp b/ov_init/src/dynamic/DynamicInitializer.cpp index e69405dcc..1fe86768a 100644 --- a/ov_init/src/dynamic/DynamicInitializer.cpp +++ b/ov_init/src/dynamic/DynamicInitializer.cpp @@ -224,6 +224,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian // We will recover position of feature, velocity, gravity // Based on Equation (14) in the following paper: // https://ieeexplore.ieee.org/abstract/document/6386235 + // State ordering is: [features, velocity, gravity] // Feature size of 1 will use the first ever bearing of the feature as true (depth only..) const bool use_single_depth = false; int size_feature = (use_single_depth) ? 1 : 3; @@ -306,6 +307,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian } // Loop through each feature observation and append it! + // State ordering is: [features, velocity, gravity] Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size); Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements); PRINT_DEBUG("[init-d]: system of %d measurement x %d states created (%d features, %s)\n", num_measurements, system_size, num_features, @@ -358,35 +360,23 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian // = R_C0toCi * R_ItoC * (p_FinI0 - v_I0inI0 * dt - 0.5 * grav_inI0 * dt^2 - alpha) + p_IinC Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3); H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1); - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, system_size); + Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk; + Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size); + Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC; if (size_feature == 1) { assert(false); - // H.block(0, size_feature * map_features.at(feat.first), 2, 1) = H_proj * R_ItoC * R_I0toIk * features_bearings.at(feat->featid); + // Substitute in p_FinI0 = z*bearing_inC0_rotI0 - R_ItoC^T*p_IinC + // H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 1) = Y * features_bearings.at(feat.first); + // b_i += Y * R_ItoC.transpose() * p_IinC; } else { - H.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = H_proj * R_ItoC * R_I0toIk; // feat + H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y; // feat } - H.block(0, size_feature * num_features + 0, 2, 3) = -H_proj * R_ItoC * R_I0toIk * DT; // vel - H.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * H_proj * R_ItoC * R_I0toIk * DT * DT; // grav - Eigen::MatrixXd b_i = H_proj * (R_ItoC * R_I0toIk * alpha_I0toIk - p_IinC); - - // Uncertainty of the measurement is the feature pixel noise and and preintegration - // TODO: We should propagate the raw pixel uncertainty to the normalized and the preintegration error to the measurement - // TODO: Fix this logic since this uses a unique H_proj matrix structure... - // Eigen::MatrixXd R_sqrtinv = 1e4 * Eigen::MatrixXd::Identity(2, 2); - // if (map_camera_cpi_I0toIi.find(time) != map_camera_cpi_I0toIi.end() && map_camera_cpi_I0toIi.at(time) != nullptr) { - // Eigen::MatrixXd H_cpi = H_proj * R_ItoC * R_I0toIk; - // Eigen::MatrixXd P_alpha = map_camera_cpi_I0toIi.at(time)->P_meas.block(12, 12, 3, 3); - // Eigen::MatrixXd R = H_cpi * P_alpha * H_cpi.transpose(); - // R_sqrtinv = R.llt().matrixL(); - // R_sqrtinv = R_sqrtinv.colPivHouseholderQr().solve(Eigen::MatrixXd::Identity(R.rows(), R.rows())); - // } - // Eigen::MatrixXd H_dz_dzn, H_dz_dzeta; - // params.camera_intrinsics.at(cam_id)->compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta); - // Eigen::MatrixXd R_pixel = std::pow(params.sigma_pix, 2) * H_dz_dzn * H_dz_dzn.transpose(); + H_i.block(0, size_feature * num_features + 0, 2, 3) = -DT * Y; // vel + H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * DT * DT * Y; // grav // Else lets append this to our system! - A.block(index_meas, 0, 2, A.cols()) = H; // R_sqrtinv * H; - b.block(index_meas, 0, 2, 1) = b_i; // R_sqrtinv * b_i; + A.block(index_meas, 0, 2, A.cols()) = H_i; + b.block(index_meas, 0, 2, 1) = b_i; index_meas += 2; } } @@ -471,7 +461,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd); Eigen::VectorXd state_grav = D_lambdaI_inv * d; - // Overwrite our state + // Overwrite our state: [features, velocity, gravity] Eigen::VectorXd state_feat_vel = -A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b; Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1); x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel; @@ -533,15 +523,17 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian Eigen::Vector3d p_FinI0; if (size_feature == 1) { assert(false); - // p_FinI0 = x_hat(size_feature * map_features.at(feat.first) + 6, 0) * features_bearings.at(feat->featid); + // double depth = x_hat(size_feature * A_index_features.at(feat.first), 0); + // p_FinI0 = depth * features_bearings.at(feat.first) - R_ItoC.transpose() * p_IinC; } else { - p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first) + 6, 0, 3, 1); + p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1); } bool is_behind = false; for (auto const &camtime : feat.second->timestamps) { size_t cam_id = camtime.first; Eigen::Vector4d q_ItoC = params.camera_extrinsics.at(cam_id).block(0, 0, 4, 1); - Eigen::Vector3d p_FinC0 = quat_2_Rot(q_ItoC) * p_FinI0; + Eigen::Vector3d p_IinC = params.camera_extrinsics.at(cam_id).block(4, 0, 3, 1); + Eigen::Vector3d p_FinC0 = quat_2_Rot(q_ItoC) * p_FinI0 + p_IinC; if (p_FinC0(2) < 0) { is_behind = true; } @@ -1013,8 +1005,8 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian ceres::Covariance::Options options_cov; options_cov.null_space_rank = (!params.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8)); options_cov.min_reciprocal_condition_number = params.init_dyn_min_rec_cond; - options_cov.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD; // SPARSE_QR, DENSE_SVD - options_cov.apply_loss_function = false; + options_cov.algorithm_type = ceres::CovarianceAlgorithmType::SPARSE_QR; // SPARSE_QR, DENSE_SVD + options_cov.apply_loss_function = true; // Better consistency if we use this options_cov.num_threads = params.init_dyn_mle_max_threads; ceres::Covariance problem_cov(options_cov); bool success = problem_cov.Compute(covariance_blocks, &problem); diff --git a/ov_init/src/test_dynamic_init.cpp b/ov_init/src/test_dynamic_init.cpp index 87196e2b3..9b20ced93 100644 --- a/ov_init/src/test_dynamic_init.cpp +++ b/ov_init/src/test_dynamic_init.cpp @@ -311,13 +311,13 @@ int main(int argc, char **argv) { sensor_msgs::PointCloud2 cloud; cloud.header.frame_id = "global"; cloud.header.stamp = ros::Time::now(); - cloud.width = 3 * _features_SLAM.size(); + cloud.width = _features_SLAM.size(); cloud.height = 1; cloud.is_bigendian = false; cloud.is_dense = false; // there may be invalid points sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * _features_SLAM.size()); + modifier.resize(_features_SLAM.size()); sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); diff --git a/ov_init/src/test_dynamic_mle.cpp b/ov_init/src/test_dynamic_mle.cpp index 42212a426..4cdea0937 100644 --- a/ov_init/src/test_dynamic_mle.cpp +++ b/ov_init/src/test_dynamic_mle.cpp @@ -614,13 +614,13 @@ int main(int argc, char **argv) { sensor_msgs::PointCloud2 cloud; cloud.header.frame_id = "global"; cloud.header.stamp = ros::Time::now(); - cloud.width = 3 * map_features.size(); + cloud.width = map_features.size(); cloud.height = 1; cloud.is_bigendian = false; cloud.is_dense = false; // there may be invalid points sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * map_features.size()); + modifier.resize(map_features.size()); sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); From ef899cda66a5388ad26d9b151cf6f41279e93e82 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 8 Jun 2023 17:41:13 -0400 Subject: [PATCH 60/60] fix 16.04 compile, default should be SPARSE_QR --- ov_init/src/dynamic/DynamicInitializer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ov_init/src/dynamic/DynamicInitializer.cpp b/ov_init/src/dynamic/DynamicInitializer.cpp index 1fe86768a..dc48a11c9 100644 --- a/ov_init/src/dynamic/DynamicInitializer.cpp +++ b/ov_init/src/dynamic/DynamicInitializer.cpp @@ -1005,8 +1005,8 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian ceres::Covariance::Options options_cov; options_cov.null_space_rank = (!params.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8)); options_cov.min_reciprocal_condition_number = params.init_dyn_min_rec_cond; - options_cov.algorithm_type = ceres::CovarianceAlgorithmType::SPARSE_QR; // SPARSE_QR, DENSE_SVD - options_cov.apply_loss_function = true; // Better consistency if we use this + // options_cov.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD; + options_cov.apply_loss_function = true; // Better consistency if we use this options_cov.num_threads = params.init_dyn_mle_max_threads; ceres::Covariance problem_cov(options_cov); bool success = problem_cov.Compute(covariance_blocks, &problem);