From 3a941a0ef40699c32360042f6cf755e1d3e03d79 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Jul 2015 00:55:34 -0700 Subject: [PATCH] NavState retract derivatives --- gtsam/navigation/PreintegrationBase.h | 31 +++++++-- gtsam/navigation/tests/testImuFactor.cpp | 87 +++++++++++++++++++----- 2 files changed, 96 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 2b61b95bb..65fafe79a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -67,12 +67,23 @@ public: static Eigen::Block dP(const Vector9& v) { return v.segment<3>(3); } static Eigen::Block dV(const Vector9& v) { return v.segment<3>(6); } - // Specialize Retract/Local that agrees with IMUFactors + // Specialized Retract/Local that agrees with IMUFactors // TODO(frank): This is a very specific retract. Talk to Luca about implications. - NavState retract(const Vector9& v, // + NavState retract(const Vector9& xi, // ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { - if (H1||H2) throw std::runtime_error("NavState::retract derivatives not implemented yet"); - return NavState(rotation().expmap(dR(v)), translation() + Point3(dP(v)), velocity() + dV(v)); + Matrix3 H1R, H2R; + const Rot3 R = rotation().expmap(dR(xi), H1 ? &H1R : 0, H2 ? &H2R : 0); + const Point3 p = translation() + Point3(dP(xi)); + const Vector v = velocity() + dV(xi); + if (H1) { + H1->setIdentity(); + H1->topLeftCorner<3,3>() = H1R; + } + if (H2) { + H2->setIdentity(); + H2->topLeftCorner<3,3>() = H2R; + } + return NavState(R, p, v); } Vector9 localCoordinates(const NavState& g, // ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { @@ -88,7 +99,17 @@ public: // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors template<> -struct traits : internal::LieGroupTraits {}; +struct traits : internal::LieGroupTraits { + static void Print(const NavState& m, const std::string& s = "") { + m.rotation().print(s+".R"); + m.translation().print(s+".P"); + print((Vector)m.velocity(),s+".V"); + } + static bool Equals(const NavState& m1, const NavState& m2, double tol = 1e-8) { + return m1.pose().equals(m2.pose(), tol) + && equal_with_abs_tol(m1.velocity(), m2.velocity(), tol); + } +}; /// @deprecated struct PoseVelocityBias { diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 70b20ef00..ad41b81a5 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -208,6 +208,55 @@ Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector(); double deltaT = 1.0; } // namespace common +/* ************************************************************************* */ +TEST( NavState, Lie ) { + // origin and zero deltas + NavState identity; + 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(zero(9), identity.localCoordinates(identity), tol)); + + NavState state1(rot, pt, vel); + 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; + Rot3 rot2 = rot.expmap(delta.head<3>()); + Point3 t2 = pt + Point3(delta.segment<3>(3)); + Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3); + NavState state2(rot2, t2, vel2); + EXPECT(assert_equal(state2, (NavState)state1.retract(delta), tol)); + EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol)); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.retract(delta); + EXPECT(assert_equal(delta, state2.localCoordinates(state3), tol)); + + // roundtrip from state3 to state4 and back, with expmap. + NavState state4 = state3.expmap(delta); + 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(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); + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( + boost::bind(&NavState::retract, state1, _1, boost::none, boost::none), delta); + EXPECT(assert_equal(eH2, aH2)); +} + /* ************************************************************************* */ TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; @@ -225,42 +274,46 @@ TEST(ImuFactor, PreintegrationBaseMethods) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); { // biasCorrectedDelta - Vector9 expected; // TODO(frank): taken from output. Should really verify. - expected << 0.0628318531, 0.0, 0.0, 4.905, -2.19885135, -8.20622494, 9.81, -4.13885394, -16.4774682; Matrix96 actualH; - EXPECT(assert_equal(expected, pim.biasCorrectedDelta(bias, actualH), 1e-5)); + pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); } { - Vector9 expected; // TODO(frank): taken from output. Should really verify. - expected << -0.0212372436, -0.0407423986, -0.0974116854, 0.1038, 0.038, -0.0804, 0.1038, 0.038, -0.0804; Matrix99 actualH; - EXPECT(assert_equal(expected, pim.integrateCoriolis(state1, actualH), 1e-5)); + pim.integrateCoriolis(state1, actualH); Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::integrateCoriolis, pim, _1, boost::none), state1); EXPECT(assert_equal(expectedH, actualH)); } { - Vector9 expected; // TODO(frank): taken from output. Should really verify. - expected << 0.0415946095, -0.0407423986, -0.0974116854, 1.1038, 0.038, 9.7096, -0.0834140265, 0.228178303, -0.0967695179; - Matrix99 actualH1, actualH2; + Matrix99 aH1, aH2; Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias); - EXPECT( - assert_equal(expected, - pim.recombinedPrediction(state1, biasCorrectedDelta, actualH1, - actualH2), 1e-5)); - Matrix expectedH1 = numericalDerivative11( + pim.recombinedPrediction(state1, biasCorrectedDelta, aH1, aH2); + Matrix eH1 = numericalDerivative11( boost::bind(&PreintegrationBase::recombinedPrediction, pim, _1, biasCorrectedDelta, boost::none, boost::none), state1); - EXPECT(assert_equal(expectedH1, actualH1)); - Matrix expectedH2 = numericalDerivative11( + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( boost::bind(&PreintegrationBase::recombinedPrediction, pim, state1, _1, boost::none, boost::none), biasCorrectedDelta); - EXPECT(assert_equal(expectedH2, actualH2)); + EXPECT(assert_equal(eH2, aH2)); + } + { + Matrix99 aH1; + Matrix96 aH2; + pim.predict(state1, bias, aH1, aH2); + Matrix eH1 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none, + boost::none), state1); + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, + boost::none), bias); + EXPECT(assert_equal(eH2, aH2)); } }