From 75581e5cc628cf6172ebc8c5dfd1005db0d3022f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Aug 2022 14:53:59 -0400 Subject: [PATCH 1/2] change from biasAccOmegaInt to biasAccOmegaInit --- examples/CombinedImuFactorsExample.cpp | 2 +- examples/ImuFactorsExample.cpp | 2 +- gtsam/navigation/CombinedImuFactor.cpp | 6 +++--- gtsam/navigation/CombinedImuFactor.h | 12 ++++++------ gtsam/navigation/tests/testCombinedImuFactor.cpp | 2 +- .../examples/ISAM2_SmartFactorStereo_IMU.cpp | 2 +- tests/testImuPreintegration.cpp | 4 ++-- 7 files changed, 15 insertions(+), 15 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index e0396ee818..35ed387c40 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -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_init; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index c176318642..12167a19d1 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -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_init; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 8d3a7dd315..0813bbfd12 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); } //------------------------------------------------------------------------------ @@ -135,7 +135,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/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index b496181eda..7e45e5380e 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 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 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 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: @@ -109,7 +109,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); } public: diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aacfff0f0d..de8a97ee1b 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -43,7 +43,7 @@ static boost::shared_ptr Params() { p->integrationCovariance = 0.0001 * I_3x3; p->biasAccCovariance = Z_3x3; p->biasOmegaCovariance = Z_3x3; - p->biasAccOmegaInt = Z_6x6; + p->biasAccOmegaInit = Z_6x6; return p; } } // namespace testing diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp index c0a102d111..27d87d217f 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; From dae04210cd0e7093d012c6ea49d702000050639b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 12 Jan 2024 17:39:31 -0500 Subject: [PATCH 2/2] leftover fixes --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/tests/testCombinedImuFactor.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 5c6be28ab0..82bb48db4e 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -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(); } diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 61e98900b3..423756e46c 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -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)); } /* ************************************************************************* */