From 103c78b0de0522ca838d444f00ab93b14aff388e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Aug 2022 10:31:10 -0400 Subject: [PATCH] revert name change, save for another PR --- 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 35ed387c40..e0396ee818 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->biasAccOmegaInit = bias_acc_omega_init; + p->biasAccOmegaInt = bias_acc_omega_init; return p; } diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 12167a19d1..c176318642 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->biasAccOmegaInit = bias_acc_omega_init; + p->biasAccOmegaInt = bias_acc_omega_init; return p; } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 0813bbfd12..8d3a7dd315 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 << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]" + cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\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(biasAccOmegaInit, e->biasAccOmegaInit, tol); + equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, 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().biasAccOmegaInit; + const Matrix6& bInitCov = p().biasAccOmegaInt; // 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 7e45e5380e..b496181eda 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 biasAccOmegaInit; ///< covariance of bias used as initial estimate. + Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate. /// Default constructor makes uninitialized params struct. /// Used for serialization. PreintegrationCombinedParams() : biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), - biasAccOmegaInit(I_6x6) {} + biasAccOmegaInt(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), biasAccOmegaInit(I_6x6) { + biasOmegaCovariance(I_3x3), biasAccOmegaInt(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) { biasAccOmegaInit=cov; } + void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; } const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInit; } + const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } 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(biasAccOmegaInit); + ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); } public: diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index de8a97ee1b..aacfff0f0d 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->biasAccOmegaInit = Z_6x6; + p->biasAccOmegaInt = 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 27d87d217f..c0a102d111 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->biasAccOmegaInit = Matrix::Identity(6, 6) * 1e-5; + p->biasAccOmegaInt = 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 94e6fb89f2..1f584be0ed 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 biasAccOmegaInit = 1e-5; + double biasAccOmegaInt = 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->biasAccOmegaInit = I_6x6 * biasAccOmegaInit; + imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt; // Initial state Pose3 priorPose;