/* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information * -------------------------------------------------------------------------- */ /** * @file testNavState.cpp * @brief Unit tests for NavState * @author Frank Dellaert * @date July 2015 */ #include #include #include #include using namespace std; using namespace gtsam; static const Rot3 kRotation = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); static const Pose3 kPose(kRotation, kPosition); static const Velocity3 kVelocity(0.4, 0.5, 0.6); static const NavState kIdentity; static const NavState kState1(kRotation, kPosition, kVelocity); static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { boost::function construct = boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::none); Matrix aH1, aH2; EXPECT( assert_equal(kState1, NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); } /* ************************************************************************* */ TEST( NavState, Attitude) { Matrix39 aH, eH; Rot3 actual = kState1.attitude(aH); EXPECT(assert_equal(actual, kRotation)); eH = numericalDerivative11( boost::bind(&NavState::attitude, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } /* ************************************************************************* */ TEST( NavState, Position) { Matrix39 aH, eH; Point3 actual = kState1.position(aH); EXPECT(assert_equal(actual, kPosition)); eH = numericalDerivative11( boost::bind(&NavState::position, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } /* ************************************************************************* */ TEST( NavState, Velocity) { Matrix39 aH, eH; Velocity3 actual = kState1.velocity(aH); EXPECT(assert_equal(actual, kVelocity)); eH = numericalDerivative11( boost::bind(&NavState::velocity, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } /* ************************************************************************* */ TEST( NavState, MatrixGroup ) { // check roundtrip conversion to 7*7 matrix representation Matrix7 T = kState1.matrix(); EXPECT(assert_equal(kState1, NavState(T))); // check group product agrees with matrix product NavState state2 = kState1 * kState1; Matrix T2 = T * T; EXPECT(assert_equal(state2, NavState(T2))); EXPECT(assert_equal(T2, state2.matrix())); } /* ************************************************************************* */ TEST( NavState, Manifold ) { // Check zero xi EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi))); EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity))); EXPECT(assert_equal(kState1, kState1.retract(kZeroXi))); EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1))); // Check definition of retract as operating on components separately Vector9 xi; xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; Rot3 drot = Rot3::Expmap(xi.head<3>()); Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); NavState state2 = kState1 * NavState(drot, dt, dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); // roundtrip from state2 to state3 and back NavState state3 = state2.retract(xi); EXPECT(assert_equal(xi, state2.localCoordinates(state3))); // Check derivatives for ChartAtOrigin::Retract Matrix9 aH; // For zero xi boost::function Retract = boost::bind( NavState::ChartAtOrigin::Retract, _1, boost::none); NavState::ChartAtOrigin::Retract(kZeroXi, aH); EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH)); // For non-zero xi NavState::ChartAtOrigin::Retract(xi, aH); EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH)); // Check derivatives for ChartAtOrigin::Local // For zero xi boost::function Local = boost::bind( NavState::ChartAtOrigin::Local, _1, boost::none); NavState::ChartAtOrigin::Local(kIdentity, aH); EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH)); // For non-zero xi NavState::ChartAtOrigin::Local(kState1, aH); EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH)); // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); boost::function retract = boost::bind(&NavState::retract, _1, _2, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); // Check localCoordinates derivatives boost::function local = boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::none); // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2)); // from identity to state2 kIdentity.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1)); EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2)); // from state2 to identity state2.localCoordinates(kIdentity, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1)); EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } /* ************************************************************************* */ TEST( NavState, Lie ) { // Check zero xi EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi))); EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity))); EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi))); EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1))); // Expmap/Logmap roundtrip Vector xi(9); xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; NavState state2 = NavState::Expmap(xi); EXPECT(assert_equal(xi, NavState::Logmap(state2))); // roundtrip from state2 to state3 and back NavState state3 = state2.expmap(xi); EXPECT(assert_equal(xi, state2.logmap(state3))); // For the expmap/logmap (not necessarily expmap/local) -xi goes other way EXPECT(assert_equal(state2, state3.expmap(-xi))); EXPECT(assert_equal(xi, -state3.logmap(state2))); } /* ************************************************************************* */ static const double dt = 2.0; boost::function coriolis = boost::bind( &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); TEST(NavState, Coriolis) { Matrix9 aH; // first-order kState1.coriolis(dt, kOmegaCoriolis, false, aH); EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH)); // second-order kState1.coriolis(dt, kOmegaCoriolis, true, aH); EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH)); } TEST(NavState, Coriolis2) { Matrix9 aH; NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); // first-order state2.coriolis(dt, kOmegaCoriolis, false, aH); EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH)); // second-order state2.coriolis(dt, kOmegaCoriolis, true, aH); EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); } /* ************************************************************************* */ TEST(NavState, correctPIM) { Vector9 xi; xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; Matrix9 aH1, aH2; boost::function correctPIM = boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, false, boost::none, boost::none); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */