From a99911b997d6563c5fd96c89e91b31d162071c65 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jul 2015 22:24:19 +0200 Subject: [PATCH] Now uses NavState::predict ! --- gtsam/navigation/PreintegrationBase.cpp | 55 +++--------------------- gtsam/navigation/PreintegrationBase.h | 5 --- gtsam/navigation/tests/testImuFactor.cpp | 46 ++++++++------------ 3 files changed, 25 insertions(+), 81 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 1e3f3520e..e2f1466a6 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -63,7 +63,7 @@ void PreintegrationBase::updatePreintegratedMeasurements( const Vector3& correctedAcc, const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - const Matrix3 dRij = deltaRij().matrix(); // expensive + const Matrix3 dRij = deltaRij_.matrix(); // expensive const Vector3 temp = dRij * correctedAcc * deltaT; if (!p().use2ndOrderIntegration) { @@ -98,9 +98,9 @@ void PreintegrationBase::updatePreintegratedMeasurements( void PreintegrationBase::updatePreintegratedJacobians( const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT) { - const Matrix3 dRij = deltaRij().matrix(); // expensive + const Matrix3 dRij = deltaRij_.matrix(); // expensive const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT - * delRdelBiasOmega(); + * delRdelBiasOmega_; if (!p().use2ndOrderIntegration) { delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; @@ -157,48 +157,6 @@ Vector9 PreintegrationBase::biasCorrectedDelta( return xi; } -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, - const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) const { - - const Rot3& rot_i = state_i.attitude(); - const Vector3& vel_i = state_i.velocity(); - const double dt = deltaTij(), dt2 = dt * dt; - - // Rotation, position, and velocity: - Vector9 xi; - Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc; - NavState::dR(xi) = NavState::dR(biasCorrectedDelta); - NavState::dP(xi) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri, - D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2; - NavState::dV(xi) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri, - D_dV_bc) + p().gravity * dt; - - Matrix9 Hcoriolis; - if (p().omegaCoriolis) { - state_i.addCoriolis(&xi, *(p().omegaCoriolis), deltaTij(), - p().use2ndOrderCoriolis, H1 ? &Hcoriolis : 0); - } - if (H1) { - H1->setZero(); - H1->block<3, 3>(3, 0) = D_dP_Ri; - H1->block<3, 3>(3, 6) = I_3x3 * dt; - H1->block<3, 3>(6, 0) = D_dV_Ri; - if (p().omegaCoriolis) - *H1 += Hcoriolis; - } - if (H2) { - H2->setZero(); - Matrix3 Ri = rot_i.matrix(); - H2->block<3, 3>(0, 0) = I_3x3; - H2->block<3, 3>(3, 3) = Ri; - H2->block<3, 3>(6, 6) = Ri; - } - - return xi; -} - //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, @@ -207,8 +165,9 @@ NavState PreintegrationBase::predict(const NavState& state_i, Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = recombinedPrediction(state_i, biasCorrected, - H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0); + Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity, + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); if (H1) @@ -300,7 +259,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, Matrix3 D_fR_fRrot; Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0); - const double dt = deltaTij(), dt2 = dt * dt; + const double dt = deltaTij_, dt2 = dt * dt; Matrix3 RitOmegaCoriolisHat = Z_3x3; if ((H1 || H2) && p().omegaCoriolis) RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 8e7841f7c..2ee6b79e6 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -152,11 +152,6 @@ class PreintegrationBase : public PreintegratedRotation { Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = boost::none) const; - /// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector - Vector9 recombinedPrediction(const NavState& state_i, - const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none) const; - /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 8ed2fb135..7bc037da9 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -213,26 +213,26 @@ double deltaT = 1.0; TEST( NavState, Lie ) { // origin and zero deltas NavState identity; - const double tol=1e-5; + const double tol = 1e-5; Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3); Point3 pt(1.0, 2.0, 3.0); Velocity3 vel(0.4, 0.5, 0.6); - EXPECT(assert_equal(identity, (NavState)identity.retract(zero(9)), tol)); + EXPECT(assert_equal(identity, (NavState )identity.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol)); NavState state1(rot, pt, vel); - EXPECT(assert_equal(state1, (NavState)state1.retract(zero(9)), tol)); + EXPECT(assert_equal(state1, (NavState )state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); // Special retract Vector delta(9); - delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; + delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; Rot3 drot = Rot3::Expmap(delta.head<3>()); - Point3 dt = Point3(delta.segment<3>(3)); + Point3 dt = Point3(delta.segment < 3 > (3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); NavState state2 = state1 * NavState(drot, dt, dvel); - EXPECT(assert_equal(state2, (NavState)state1.retract(delta), tol)); + EXPECT(assert_equal(state2, (NavState )state1.retract(delta), tol)); EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol)); // roundtrip from state2 to state3 and back @@ -244,17 +244,19 @@ TEST( NavState, Lie ) { EXPECT(assert_equal(delta, state3.logmap(state4), tol)); // For the expmap/logmap (not necessarily retract/local) -delta goes other way - EXPECT(assert_equal(state3, (NavState)state4.expmap(-delta), tol)); + EXPECT(assert_equal(state3, (NavState )state4.expmap(-delta), tol)); EXPECT(assert_equal(delta, -state4.logmap(state3), tol)); // retract derivatives Matrix9 aH1, aH2; state1.retract(delta, aH1, aH2); Matrix eH1 = numericalDerivative11( - boost::bind(&NavState::retract, _1, delta, boost::none, boost::none),state1); + boost::bind(&NavState::retract, _1, delta, boost::none, boost::none), + state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - boost::bind(&NavState::retract, state1, _1, boost::none, boost::none), delta); + boost::bind(&NavState::retract, state1, _1, boost::none, boost::none), + delta); EXPECT(assert_equal(eH2, aH2)); } @@ -282,19 +284,6 @@ TEST(ImuFactor, PreintegrationBaseMethods) { boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); } - { - Matrix9 aH1, aH2; - Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias); - pim.recombinedPrediction(state1, biasCorrectedDelta, aH1, aH2); - Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::recombinedPrediction, pim, _1, - biasCorrectedDelta, boost::none, boost::none), state1); - EXPECT(assert_equal(eH1, aH1)); - Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::recombinedPrediction, pim, state1, _1, - boost::none, boost::none), biasCorrectedDelta); - EXPECT(assert_equal(eH2, aH2)); - } { Matrix9 aH1; Matrix96 aH2; @@ -302,11 +291,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) { Matrix eH1 = numericalDerivative11( boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none, boost::none), state1); - EXPECT(assert_equal(eH1, aH1)); + EXPECT(assert_equal(eH1, aH1, 1e-8)); Matrix eH2 = numericalDerivative11( boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, boost::none), bias); - EXPECT(assert_equal(eH2, aH2)); + EXPECT(assert_equal(eH2, aH2, 1e-8)); } } @@ -314,9 +303,9 @@ TEST(ImuFactor, PreintegrationBaseMethods) { TEST(ImuFactor, ErrorAndJacobians) { using namespace common; bool use2ndOrderIntegration = true; - PreintegratedImuMeasurements pim(bias, - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, use2ndOrderIntegration); + PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, + use2ndOrderIntegration); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -933,7 +922,8 @@ TEST(ImuFactor, PredictRotation) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, kZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + kZeroOmegaCoriolis); // Predict Pose3 x1, x2;