From 5cf98313bd957f5d662a22875a8a5c5f7ee2d7ad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2016 13:43:46 -0800 Subject: [PATCH] Combined two methods and renamed static method --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/PreintegrationBase.cpp | 56 ++++++++----------- gtsam/navigation/PreintegrationBase.h | 25 ++++----- .../tests/testPreintegrationBase.cpp | 6 +- 5 files changed, 38 insertions(+), 53 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 9c266e7a1..db0e6d34a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -70,7 +70,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Update preintegrated measurements. Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C); + updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 626b2f5c5..c87df4c37 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -55,7 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C); + updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c96e39c9b..c31120ffc 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -116,7 +116,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( //------------------------------------------------------------------------------ // See extensive discussion in ImuFactor.lyx -Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body, +Vector9 PreintegrationBase::UpdatePreintegrated(const Vector3& a_body, const Vector3& w_body, double dt, const Vector9& preintegrated, OptionalJacobian<9, 9> A, @@ -178,43 +178,31 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body, } //------------------------------------------------------------------------------ -Vector9 PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt, Matrix9* A, - Matrix93* B, Matrix93* C) const { +void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt, Matrix9* A, + Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame - Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega); + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - if (!p().body_P_sensor) { - return UpdateEstimate(unbiasedAcc, unbiasedOmega, dt, preintegrated_, A, B, - C); - } else { - // More complicated derivatives in case of sensor displacement - Matrix3 D_correctedAcc_unbiasedAcc, D_correctedAcc_unbiasedOmega, - D_correctedOmega_unbiasedOmega; - auto corrected = correctMeasurementsBySensorPose( - unbiasedAcc, unbiasedOmega, D_correctedAcc_unbiasedAcc, - D_correctedAcc_unbiasedOmega, D_correctedOmega_unbiasedOmega); + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - const Vector9 updated = UpdateEstimate(corrected.first, corrected.second, dt, - preintegrated_, A, B, C); - - *C *= D_correctedOmega_unbiasedOmega; - if (!p().body_P_sensor->translation().vector().isZero()) - *C += *B* D_correctedAcc_unbiasedOmega; - *B *= D_correctedAcc_unbiasedAcc; // NOTE(frank): needs to be last - return updated; - } -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::update(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt, - Matrix9* A, Matrix93* B, Matrix93* C) { - // Do update + // Do update deltaTij_ += dt; - preintegrated_ = updatedPreintegrated(measuredAcc, measuredOmega, dt, A, B, C); + preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().vector().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 2f3e9cd41..70b12ccdb 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -172,23 +172,20 @@ public: OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const; // Update integrated vector on tangent manifold preintegrated with acceleration - // readings a_body and gyro readings w_body, bias-corrected in body frame. - static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body, - double dt, const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); - - /// Calculate the updated preintegrated measurement, does not modify - /// It takes measured quantities in the j frame - Vector9 updatedPreintegrated(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt, - Matrix9* A, Matrix93* B, Matrix93* C) const; + // Static, functional version. + static Vector9 UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, double dt, + const Vector9& preintegrated, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - void update(const Vector3& measuredAcc, const Vector3& measuredOmega, - const double deltaT, Matrix9* A, Matrix93* B, Matrix93* C); + /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose + void updatedPreintegrated(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double deltaT, + Matrix9* A, Matrix93* B, Matrix93* C); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index 3327bfdb6..e9b611ef0 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -39,7 +39,7 @@ static boost::shared_ptr defaultParams() { } Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - return PreintegrationBase::UpdateEstimate(a, w, kDt, zeta); + return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); } /* ************************************************************************* */ @@ -50,7 +50,7 @@ TEST(PreintegrationBase, UpdateEstimate1) { zeta.setZero(); Matrix9 aH1; Matrix93 aH2, aH3; - pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3); + pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3); EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); @@ -64,7 +64,7 @@ TEST(PreintegrationBase, UpdateEstimate2) { zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; Matrix9 aH1; Matrix93 aH2, aH3; - pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3); + pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));