diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 6ce4f76faa..b04dcc094a 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -123,7 +123,7 @@ std::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_init; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 342f1f2203..80bf21d75f 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -110,7 +110,7 @@ std::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_init; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2eb9e0c08..82bb48db4e 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -41,7 +41,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; } @@ -54,7 +54,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); } //------------------------------------------------------------------------------ @@ -84,7 +84,7 @@ void PreintegratedCombinedMeasurements::resetIntegration( const gtsam::Matrix6& Q_init) { // Base class method to reset the preintegrated measurements PreintegrationType::resetIntegration(); - p().biasAccOmegaInt = Q_init; + p().biasAccOmegaInit = Q_init; preintMeasCov_.setZero(); } @@ -147,7 +147,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( const Matrix3& aCov = p().accelerometerCovariance; const Matrix3& wCov = p().gyroscopeCovariance; const Matrix3& iCov = p().integrationCovariance; - const Matrix6& bInitCov = p().biasAccOmegaInt; + const Matrix6& bInitCov = p().biasAccOmegaInit; // first order uncertainty propagation // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose() diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h index 6be05c082a..e869e3cb3d 100644 --- a/gtsam/navigation/PreintegrationCombinedParams.h +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -38,14 +38,14 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { ///< accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing ///< gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate. + 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 @@ -53,7 +53,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { : PreintegrationParams(n_gravity_), biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), - biasAccOmegaInt(I_6x6) {} + biasAccOmegaInit(I_6x6) {} // Default Params for a Z-down navigation frame, such as NED: gravity points // along positive Z-axis @@ -77,11 +77,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance = cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance = cov; } - void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt = cov; } + void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInit = cov; } const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } + const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInit; } private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION @@ -93,7 +93,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { 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); } #endif diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c4fefb8ffa..423756e46c 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -39,14 +39,14 @@ namespace testing { static std::shared_ptr Params( const Matrix3& biasAccCovariance = Matrix3::Zero(), const Matrix3& biasOmegaCovariance = Matrix3::Zero(), - const Matrix6& biasAccOmegaInt = Matrix6::Zero()) { + const Matrix6& biasAccOmegaInit = Matrix6::Zero()) { auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->integrationCovariance = 0.0001 * I_3x3; p->biasAccCovariance = biasAccCovariance; p->biasOmegaCovariance = biasOmegaCovariance; - p->biasAccOmegaInt = biasAccOmegaInt; + p->biasAccOmegaInit = biasAccOmegaInit; return p; } } // namespace testing @@ -349,12 +349,12 @@ TEST(CombinedImuFactor, ResetIntegration) { // Test default method pim.resetIntegration(); Matrix6 expected = 0.1 * I_6x6; - EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9)); + EXPECT(assert_equal(expected, pim.p().biasAccOmegaInit, 1e-9)); // Test method where Q_init is provided Matrix6 expected_Q_init = I_6x6 * 0.001; pim2.resetIntegration(expected_Q_init); - EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInt, 1e-9)); + EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInit, 1e-9)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp index e386add768..d4031fd708 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 1f584be0ed..94e6fb89f2 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;