From 38e43ce0af2141c5395d6944fb1a06f54e0e508a Mon Sep 17 00:00:00 2001 From: Sandro Berchier Date: Sun, 28 Jul 2019 16:01:49 -0400 Subject: [PATCH 1/5] Added non-default const. for AHRS pre-int. and params --- gtsam/navigation/AHRSFactor.h | 22 +++++++++++++++++ gtsam/navigation/PreintegratedRotation.h | 7 ++++++ gtsam/navigation/tests/testAHRSFactor.cpp | 29 +++++++++++++++++++++++ 3 files changed, 58 insertions(+) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 8ed695622..c00fb8caf 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -55,6 +55,28 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation resetIntegration(); } + /** + * Non-Default constructor, initialize with measurements + * @param p: Parameters for AHRS pre-integration + * @param bias_hat: Current estimate of acceleration and rotation rate biases + * @param deltaTij: Delta time in pre-integration + * @param deltaRij: Delta rotation in pre-integration + * @param delRdelBiasOmega: Jacobian of rotation wrt. to gyro bias + * @param preint_meas_cov: Pre-integration covariance + */ + PreintegratedAhrsMeasurements( + const boost::shared_ptr& p, + const Vector3& bias_hat, + double deltaTij, + const Rot3& deltaRij, + const Matrix3& delRdelBiasOmega, + const Matrix3& preint_meas_cov) : + biasHat_(bias_hat), + PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega), + preintMeasCov_(preint_meas_cov) { + p_->gyroscopeCovariance = p->getGyroscopeCovariance(); + } + const Params& p() const { return *boost::static_pointer_cast(p_);} const Vector3& biasHat() const { return biasHat_; } const Matrix3& preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index bf2f5c0c8..0d68a6e18 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,6 +35,13 @@ struct GTSAM_EXPORT PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + PreintegratedRotationParams(Matrix3 gyroscope_covariance, + boost::optional omega_coriolis) + : gyroscopeCovariance(gyroscope_covariance) { + if (omega_coriolis) + omegaCoriolis.reset(omega_coriolis.get()); + } + virtual ~PreintegratedRotationParams() {} virtual void print(const std::string& s) const; diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 208d0e709..a5fae42c6 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -119,6 +119,35 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } +//****************************************************************************** +TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { + // Linearization point + Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; + Vector3 omegaCoriolis(0.1, 0.5, 0.9); + PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); + Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias + Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); + double deltaTij = 0.02; + Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; + Matrix3 preintMeasCov = Matrix3::Ones()*0.2; + gtsam::PreintegratedAhrsMeasurements test_pim( + boost::make_shared(params), + bias, + deltaTij, + deltaRij, + delRdelBiasOmega, + preintMeasCov); + EXPECT(assert_equal(gyroscopeCovariance, + test_pim.p().getGyroscopeCovariance(), 1e-6)); + EXPECT(assert_equal(omegaCoriolis, + test_pim.p().getOmegaCoriolis().get(), 1e-6)); + EXPECT(assert_equal(bias, test_pim.biasHat(), 1e-6)); + DOUBLES_EQUAL(deltaTij, test_pim.deltaTij(), 1e-6); + EXPECT(assert_equal(deltaRij, Rot3(test_pim.deltaRij()), 1e-6)); + EXPECT(assert_equal(delRdelBiasOmega, test_pim.delRdelBiasOmega(), 1e-6)); + EXPECT(assert_equal(preintMeasCov, test_pim.preintMeasCov(), 1e-6)); +} + /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point From 04b9bf23e536d482f6d1feea968ebe956bca0896 Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 30 Jul 2019 11:41:14 -0400 Subject: [PATCH 2/5] Remove redundant gyroCov param update in ctor --- gtsam/navigation/AHRSFactor.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index c00fb8caf..02384e23d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -73,9 +73,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation const Matrix3& preint_meas_cov) : biasHat_(bias_hat), PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega), - preintMeasCov_(preint_meas_cov) { - p_->gyroscopeCovariance = p->getGyroscopeCovariance(); - } + preintMeasCov_(preint_meas_cov) {} const Params& p() const { return *boost::static_pointer_cast(p_);} const Vector3& biasHat() const { return biasHat_; } From 56cc2b037fb43bcdf5fd3f946cdd760d1361c2fd Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 30 Jul 2019 11:43:08 -0400 Subject: [PATCH 3/5] Use const& for input param --- gtsam/navigation/PreintegratedRotation.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 0d68a6e18..71ef5d08f 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,8 +35,8 @@ struct GTSAM_EXPORT PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} - PreintegratedRotationParams(Matrix3 gyroscope_covariance, - boost::optional omega_coriolis) + PreintegratedRotationParams(const Matrix3& gyroscope_covariance, + boost::optional omega_coriolis) : gyroscopeCovariance(gyroscope_covariance) { if (omega_coriolis) omegaCoriolis.reset(omega_coriolis.get()); From 162c9ab472d34931931d92950c512ef64d5d2241 Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 30 Jul 2019 11:44:33 -0400 Subject: [PATCH 4/5] Remove outdated comment --- gtsam/navigation/tests/testAHRSFactor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index a5fae42c6..c89019cff 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -121,7 +121,6 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { //****************************************************************************** TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { - // Linearization point Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; Vector3 omegaCoriolis(0.1, 0.5, 0.9); PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); From 3e83f00788d11faeeb929c36b9eeaa7b4b684683 Mon Sep 17 00:00:00 2001 From: Toni Date: Tue, 30 Jul 2019 11:44:58 -0400 Subject: [PATCH 5/5] Rename test_pim to actualPim --- gtsam/navigation/tests/testAHRSFactor.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index c89019cff..d32553b94 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -129,22 +129,22 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { double deltaTij = 0.02; Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; Matrix3 preintMeasCov = Matrix3::Ones()*0.2; - gtsam::PreintegratedAhrsMeasurements test_pim( - boost::make_shared(params), + PreintegratedAhrsMeasurements actualPim( + boost::make_shared(params), bias, deltaTij, deltaRij, delRdelBiasOmega, preintMeasCov); EXPECT(assert_equal(gyroscopeCovariance, - test_pim.p().getGyroscopeCovariance(), 1e-6)); + actualPim.p().getGyroscopeCovariance(), 1e-6)); EXPECT(assert_equal(omegaCoriolis, - test_pim.p().getOmegaCoriolis().get(), 1e-6)); - EXPECT(assert_equal(bias, test_pim.biasHat(), 1e-6)); - DOUBLES_EQUAL(deltaTij, test_pim.deltaTij(), 1e-6); - EXPECT(assert_equal(deltaRij, Rot3(test_pim.deltaRij()), 1e-6)); - EXPECT(assert_equal(delRdelBiasOmega, test_pim.delRdelBiasOmega(), 1e-6)); - EXPECT(assert_equal(preintMeasCov, test_pim.preintMeasCov(), 1e-6)); + actualPim.p().getOmegaCoriolis().get(), 1e-6)); + EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6)); + DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6); + EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6)); + EXPECT(assert_equal(delRdelBiasOmega, actualPim.delRdelBiasOmega(), 1e-6)); + EXPECT(assert_equal(preintMeasCov, actualPim.preintMeasCov(), 1e-6)); } /* ************************************************************************* */