diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 49cdb6835..35ed387c4 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -107,7 +107,7 @@ boost::shared_ptr imuParams() { I_3x3 * 1e-8; // error committed in integrating position from velocities Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = + Matrix66 bias_acc_omega_init = I_6x6 * 1e-5; // error in the bias used for preintegration auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); @@ -123,7 +123,7 @@ boost::shared_ptr imuParams() { // PreintegrationCombinedMeasurements params: p->biasAccCovariance = bias_acc_cov; // acc bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 38ee4c7c7..12167a19d 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -94,7 +94,7 @@ boost::shared_ptr imuParams() { I_3x3 * 1e-8; // error committed in integrating position from velocities Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = + Matrix66 bias_acc_omega_init = I_6x6 * 1e-5; // error in the bias used for preintegration auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); @@ -110,7 +110,7 @@ boost::shared_ptr imuParams() { // PreintegrationCombinedMeasurements params: p->biasAccCovariance = bias_acc_cov; // acc bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index de4d3f66d..5d49e4c0b 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const { << endl; cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]" << endl; - cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]" + cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]" << endl; } @@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth tol) && equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance, tol) && - equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol); + equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol); } //------------------------------------------------------------------------------ @@ -137,8 +137,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); - Matrix3 aCov_updated = aCov + p().biasAccOmegaInt.block<3, 3>(0, 0); - Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3); + Matrix3 aCov_updated = aCov + p().biasAccOmegaInit.block<3, 3>(0, 0); + Matrix3 wCov_updated = wCov + p().biasAccOmegaInit.block<3, 3>(3, 3); // BLOCK DIAGONAL TERMS D_t_t(&G_measCov_Gt) = (pos_H_biasAcc // @@ -156,7 +156,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInit.block<3, 3>(3, 0) * theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose(); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ce169c1d0..5849f6f24 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -62,19 +62,19 @@ typedef ManifoldPreintegration PreintegrationType; struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration + Matrix6 biasAccOmegaInit; ///< covariance of bias used as initial estimate. /// Default constructor makes uninitialized params struct. /// Used for serialization. PreintegrationCombinedParams() : biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), - biasAccOmegaInt(I_6x6) {} + biasAccOmegaInit(I_6x6) {} /// See two named constructors below for good values of n_gravity in body frame PreintegrationCombinedParams(const Vector3& n_gravity) : PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), - biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { + biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) { } @@ -93,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } - void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } + void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=cov; } const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } + const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; } private: @@ -109,7 +109,7 @@ private: ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); + ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit); } public: diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp index c0a102d11..27d87d217 100644 --- a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -57,7 +57,7 @@ struct IMUHelper { p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous p->biasOmegaCovariance = I_3x3 * pow(0.001, 2.0); // gyro bias in continuous - p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5; + p->biasAccOmegaInit = Matrix::Identity(6, 6) * 1e-5; // body to IMU rotation Rot3 iRb(0.036129, -0.998727, 0.035207, diff --git a/tests/testImuPreintegration.cpp b/tests/testImuPreintegration.cpp index 1f584be0e..94e6fb89f 100644 --- a/tests/testImuPreintegration.cpp +++ b/tests/testImuPreintegration.cpp @@ -43,7 +43,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) { double gyrNoiseSigma = 0.000208; double gyrBiasRwSigma = 0.000004; double integrationCovariance = 1e-8; - double biasAccOmegaInt = 1e-5; + double biasAccOmegaInit = 1e-5; double gravity = 9.81; double rate = 400.0; // Hz @@ -76,7 +76,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) { imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2); imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2); imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance; - imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt; + imuPreintegratedParams->biasAccOmegaInit = I_6x6 * biasAccOmegaInit; // Initial state Pose3 priorPose;