From 3af7e80f9720503b252d246893b2222b5e6bc150 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2016 09:52:13 -0800 Subject: [PATCH] Derivatives match! --- gtsam/navigation/PreintegrationBase.cpp | 22 ------------- .../tests/testCombinedImuFactor.cpp | 32 +++++++++---------- gtsam/navigation/tests/testImuFactor.cpp | 12 +++---- 3 files changed, 21 insertions(+), 45 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 269e16390..2a64074ff 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -221,9 +221,6 @@ void PreintegrationBase::update(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C) { - // Save current rotation for updating Jacobians - const Rot3 oldRij = deltaRij(); - // Do update deltaTij_ += dt; deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C); @@ -235,7 +232,6 @@ void PreintegrationBase::update(const Vector3& measuredAcc, delPdelBiasAcc_ = D_plus_abias.middleRows<3>(3); delVdelBiasAcc_ = D_plus_abias.middleRows<3>(6); -#ifdef USE_NEW_WAY_FOR_OMEGA // D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias Matrix93 D_zeta_wbias, D_plus_wbias; D_zeta_wbias << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_; @@ -243,24 +239,6 @@ void PreintegrationBase::update(const Vector3& measuredAcc, delRdelBiasOmega_ = D_plus_wbias.middleRows<3>(0); delPdelBiasOmega_ = D_plus_wbias.middleRows<3>(3); delVdelBiasOmega_ = D_plus_wbias.middleRows<3>(6); -#else - // The old way matches the derivatives of deltaRij() - Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); - - Matrix3 D_acc_R; - double dt22 = 0.5 * dt * dt; - oldRij.rotate(correctedAcc, D_acc_R); - const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; - const Vector3 integratedOmega = correctedOmega * dt; - const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); - const Matrix3 incrRt = incrR.transpose(); - - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt; - delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; - delVdelBiasOmega_ += D_acc_biasOmega * dt; -#endif } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c1dbb7c6c..2a4e00048 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -39,6 +39,10 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +typedef imuBias::ConstantBias Bias; +static const Vector3 Z_3x1 = Vector3::Zero(); +static const Bias kZeroBiasHat, kZeroBias; + namespace { // Auxiliary functions to test preintegrated Jacobians @@ -73,14 +77,6 @@ Vector3 evaluatePreintegratedMeasurementsVelocity( deltaTs).deltaVij(); } -Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaRij()); -} - } /* ************************************************************************* */ @@ -197,6 +193,17 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + // Check derivative of rotation + boost::function theta = [=]( + const Vector3& a, const Vector3& w) { + return evaluatePreintegratedMeasurements( + Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta(); + }; + EXPECT( + assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3))); + EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1), + pim.delRdelBiasOmega(), 1e-7)); + // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( @@ -212,20 +219,11 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = - numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); - Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); - // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(),1e-8)); EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(),1e-8)); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 853860516..0829fbc37 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -34,19 +34,19 @@ using namespace std; using namespace gtsam; -typedef imuBias::ConstantBias Bias; - // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +typedef imuBias::ConstantBias Bias; +static const Vector3 Z_3x1 = Vector3::Zero(); +static const Bias kZeroBiasHat, kZeroBias; + static const double kGravity = 10; static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); static const Vector3 kZeroOmegaCoriolis(0, 0, 0); static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); -static const Bias kZeroBiasHat, kZeroBias; -static const Vector3 Z_3x1 = Vector3::Zero(); /* ************************************************************************* */ namespace { @@ -500,10 +500,10 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { deltaTs); // Check derivative of rotation - boost::function theta = [=]( + boost::function theta = [=]( const Vector3& a, const Vector3& w) { return evaluatePreintegratedMeasurements( - Bias(a, w), measuredAccs, measuredOmegas, deltaTs).deltaRij(); + Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta(); }; EXPECT( assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));