From 4c8c669d71bdb080c067ae87f948b5ab7ef13c08 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Jul 2015 11:23:24 -0700 Subject: [PATCH] Moved NavState to its own header and totally revamped as a semi-direct product --- gtsam/navigation/NavState.cpp | 120 +++++++++++++ gtsam/navigation/NavState.h | 225 ++++++++++++++++++++++++ gtsam/navigation/PreintegrationBase.cpp | 10 +- gtsam/navigation/PreintegrationBase.h | 88 +-------- gtsam/navigation/tests/testNavState.cpp | 124 +++++++++++++ 5 files changed, 475 insertions(+), 92 deletions(-) create mode 100644 gtsam/navigation/NavState.cpp create mode 100644 gtsam/navigation/NavState.h create mode 100644 gtsam/navigation/tests/testNavState.cpp diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp new file mode 100644 index 000000000..6a8977fd5 --- /dev/null +++ b/gtsam/navigation/NavState.cpp @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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 NavState.h + * @brief Navigation state composing of attitude, position, and velocity + * @author Frank Dellaert + * @date July 2015 + **/ + +#include + +namespace gtsam { + +#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; + +Matrix7 NavState::matrix() const { + Matrix3 R = this->R(); + Matrix7 T; + T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + return T; +} + +void NavState::print(const std::string& s) const { + attitude().print(s + ".R"); + position().print(s + ".p"); + gtsam::print((Vector) v_, s + ".v"); +} + +bool NavState::equals(const NavState& other, double tol) const { + return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + && equal_with_abs_tol(v_, other.v_, tol); +} + +NavState NavState::inverse() const { + TIE(nRb, n_t, n_v, *this); + const Rot3 bRn = nRb.inverse(); + return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); +} + +NavState NavState::operator*(const NavState& bTc) const { + TIE(nRb, n_t, n_v, *this); + TIE(bRc, b_t, b_v, bTc); + return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); +} + +NavState::PositionAndVelocity // +NavState::operator*(const PositionAndVelocity& b_tv) const { + TIE(nRb, n_t, n_v, *this); + const Point3& b_t = b_tv.first; + const Velocity3& b_v = b_tv.second; + return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); +} + +Point3 NavState::operator*(const Point3& b_t) const { + return Point3(R_ * b_t + t_); +} + +Velocity3 NavState::operator*(const Velocity3& b_v) const { + return Velocity3(R_ * b_v + v_); +} + +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + OptionalJacobian<9, 9> H) { + Matrix3 D_R_xi; + const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0); + const Point3 p = Point3(dP(xi)); + const Vector v = dV(xi); + const NavState result(R, p, v); + if (H) { + *H << D_R_xi, Z_3x3, Z_3x3, // + Z_3x3, R.transpose(), Z_3x3, // + Z_3x3, Z_3x3, R.transpose(); + } + return result; +} + +Vector9 NavState::ChartAtOrigin::Local(const NavState& x, + OptionalJacobian<9, 9> H) { + if (H) + throw std::runtime_error( + "NavState::ChartOrigin::Local derivative not implemented yet"); + Vector9 xi; + xi << Rot3::Logmap(x.R_), x.t(), x.v(); + return xi; +} + +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { + if (H) + throw std::runtime_error("NavState::Expmap derivative not implemented yet"); + // use brute force matrix exponential + return expm(xi); +} + +Vector9 NavState::Logmap(const NavState& p, OptionalJacobian<9, 9> H) { + if (H) + throw std::runtime_error("NavState::Logmap derivative not implemented yet"); + return Vector9::Zero(); +} + +Matrix9 NavState::AdjointMap() const { + throw std::runtime_error("NavState::AdjointMap not implemented yet"); +} + +Matrix7 NavState::wedge(const Vector9& xi) { + const Matrix3 Omega = skewSymmetric(dR(xi)); + Matrix7 T; + T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0; + return T; +} + +} /// namespace gtsam diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h new file mode 100644 index 000000000..e7aab4b7c --- /dev/null +++ b/gtsam/navigation/NavState.h @@ -0,0 +1,225 @@ +/* ---------------------------------------------------------------------------- + + * 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 NavState.h + * @brief Navigation state composing of attitude, position, and velocity + * @author Frank Dellaert + * @date July 2015 + **/ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/// Velocity is currently typedef'd to Vector3 +typedef Vector3 Velocity3; + +/** + * Navigation state: Pose (rotation, translation) + velocity + * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity + */ +class NavState: public LieGroup { +private: + Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav + Point3 t_; ///< position n_t, in nav frame + Velocity3 v_; ///< velocity n_v in nav frame + +public: + + typedef std::pair PositionAndVelocity; + + /// @name Constructors + /// @{ + + /// Default constructor + NavState() : + v_(Vector3::Zero()) { + } + /// Construct from attitude, position, velocity + NavState(const Rot3& R, const Point3& t, const Velocity3& v) : + R_(R), t_(t), v_(v) { + } + /// Construct from pose and velocity + NavState(const Pose3& pose, const Velocity3& v) : + R_(pose.rotation()), t_(pose.translation()), v_(v) { + } + /// Construct from Matrix group representation (no checking) + NavState(const Matrix7& T) : + R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) { + } + /// Construct from SO(3) and R^6 + NavState(const Matrix3& R, const Vector9 tv) : + R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { + } + + /// @} + /// @name Component Access + /// @{ + + const Rot3& attitude() const { + return R_; + } + const Point3& position() const { + return t_; + } + const Velocity3& velocity() const { + return v_; + } + const Pose3 pose() const { + return Pose3(attitude(), position()); + } + + /// @} + /// @name Derived quantities + /// @{ + + /// Return rotation matrix. Induces computation in quaternion mode + Matrix3 R() const { + return R_.matrix(); + } + /// Return position as Vector3 + Vector3 t() const { + return t_.vector(); + } + /// Return velocity as Vector3. Computation-free. + const Vector3& v() const { + return v_; + } + /// Return quaternion. Induces computation in matrix mode + Quaternion quaternion() const { + return R_.toQuaternion(); + } + /// Return matrix group representation, in MATLAB notation: + /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] + /// With this embedding in GL(3), matrix product agrees with compose + Matrix7 matrix() const; + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// equals + bool equals(const NavState& other, double tol = 1e-8) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static NavState identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + /// Group compose is the semi-direct product as specified below: + /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v) + NavState operator*(const NavState& bTc) const; + + /// Native group action is on position/velocity pairs *in the body frame* as follows: + /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v) + PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const; + + /// Act on position alone, n_t = nRb * b_t + n_t + Point3 operator*(const Point3& b_t) const; + + /// Act on velocity alone, n_v = nRb * b_v + n_v + Velocity3 operator*(const Velocity3& b_v) const; + + /// @} + /// @name Manifold + /// @{ + + // Tangent space sugar. + // TODO(frank): move to private navstate namespace in cpp + static Eigen::Block dR(Vector9& v) { + return v.segment<3>(0); + } + static Eigen::Block dP(Vector9& v) { + return v.segment<3>(3); + } + static Eigen::Block dV(Vector9& v) { + return v.segment<3>(6); + } + static Eigen::Block dR(const Vector9& v) { + return v.segment<3>(0); + } + static Eigen::Block dP(const Vector9& v) { + return v.segment<3>(3); + } + static Eigen::Block dV(const Vector9& v) { + return v.segment<3>(6); + } + + // Chart at origin, constructs components separately (as opposed to Expmap) + struct ChartAtOrigin { + static NavState Retract(const Vector9& xi, // + OptionalJacobian<9, 9> H = boost::none); + static Vector9 Local(const NavState& x, // + OptionalJacobian<9, 9> H = boost::none); + }; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map at identity - create a NavState from canonical coordinates + static NavState Expmap(const Vector9& xi, // + OptionalJacobian<9, 9> H = boost::none); + + /// Log map at identity - return the canonical coordinates for this NavState + static Vector9 Logmap(const NavState& p, // + OptionalJacobian<9, 9> H = boost::none); + + /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms + /// it to a tangent vector at idenity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); + Matrix9 AdjointMap() const; + + /// wedge creates Lie algebra element from tangent vector + static Matrix7 wedge(const Vector9& xi); + + /// @} + +private: + /// @{ + /// serialization + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + ar & BOOST_SERIALIZATION_NVP(v_); + } + /// @} +}; + +// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors +template<> +struct traits : Testable, internal::LieGroupTraits { +}; + +// Partial specialization of wedge +// TODO: deprecate, make part of traits +template<> +inline Matrix wedge(const Vector& xi) { + return NavState::wedge(xi); +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 28d2c7842..d2775bfb2 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -182,7 +182,7 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i, const Pose3& pose_i = state_i.pose(); const Vector3& vel_i = state_i.velocity(); const Matrix3 Ri = pose_i.rotation().matrix(); - const Vector3& t_i = state_i.translation().vector(); + const Vector3& t_i = state_i.position().vector(); const double dt = deltaTij(), dt2 = dt * dt; const Vector3& omegaCoriolis = *p().omegaCoriolis; @@ -220,7 +220,7 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, const Matrix3 Ri = pose_i.rotation().matrix(); const double dt = deltaTij(), dt2 = dt * dt; - // Rotation, translation, and velocity: + // Rotation, position, and velocity: Vector9 delta; Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc; NavState::dR(delta) = NavState::dR(biasCorrectedDelta); @@ -274,18 +274,18 @@ NavState PreintegrationBase::predict(const NavState& state_i, static Vector9 computeError(const NavState& state_i, const NavState& state_j, const NavState& predictedState_j) { - const Rot3& rot_i = state_i.rotation(); + const Rot3& rot_i = state_i.attitude(); const Matrix Ri = rot_i.matrix(); // Residual rotation error // TODO: this also seems to be flipped from localCoordinates - const Rot3 fRrot = predictedState_j.rotation().between(state_j.rotation()); + const Rot3 fRrot = predictedState_j.attitude().between(state_j.attitude()); const Vector3 fR = Rot3::Logmap(fRrot); // Evaluate residual error, according to [3] // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance const Vector3 fp = Ri.transpose() - * (state_j.translation() - predictedState_j.translation()).vector(); + * (state_j.position() - predictedState_j.position()).vector(); // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance const Vector3 fv = Ri.transpose() diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index b601015bf..359db8c83 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -22,97 +22,11 @@ #pragma once #include +#include #include -#include -#include -#include -#include namespace gtsam { -/// Velocity in 3D is just a Vector3 -typedef Vector3 Velocity3; - -/** - * Navigation state: Pose (rotation, translation) + velocity - */ -class NavState: public ProductLieGroup { -protected: - typedef ProductLieGroup Base; - typedef OptionalJacobian<9, 9> ChartJacobian; - using Base::first; - using Base::second; - -public: - // constructors - NavState() {} - NavState(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {} - NavState(const Rot3& rot, const Point3& t, const Velocity3& vel): Base(Pose3(rot, t), vel) {} - NavState(const Base& product) : Base(product) {} - - // access - const Pose3& pose() const { return first; } - const Point3& translation() const { return pose().translation(); } - const Rot3& rotation() const { return pose().rotation(); } - const Velocity3& velocity() const { return second; } - - /// @name Manifold - /// @{ - - // NavState tangent space sugar. - static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } - static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } - static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } - static Eigen::Block dR(const Vector9& v) { return v.segment<3>(0); } - static Eigen::Block dP(const Vector9& v) { return v.segment<3>(3); } - static Eigen::Block dV(const Vector9& v) { return v.segment<3>(6); } - - // Retract/Local that creates a de-novo Nav-state from xi, on all components - // separately, but then composes with this, i.e., xi is in rotated frame! - // NOTE(frank): This agrees with Pose3.retract, in non-EXPMAP mode, treating V like P - NavState retract(const Vector9& xi, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { - Matrix3 D_R_xi; - const Rot3 R = Rot3::Expmap(dR(xi), H2 ? &D_R_xi : 0); - const Point3 p = Point3(dP(xi)); - const Vector v = dV(xi); - const NavState delta(R, p, v); - // retract only depends on this via compose, and second derivative in delta is I_9x9 - const NavState result = this->compose(delta, H1); - if (H2) { - *H2 << D_R_xi, Z_3x3, Z_3x3, // - Z_3x3, I_3x3, Z_3x3, // - Z_3x3, Z_3x3, I_3x3; - } - return result; -} - Vector9 localCoordinates(const NavState& g, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { - if (H1||H2) throw std::runtime_error("NavState::localCoordinates derivatives not implemented yet"); - const NavState delta = this->between(g); - Vector9 v; - dR(v) = Rot3::Logmap(delta.rotation()); - dP(v) = delta.translation().vector(); - dV(v) = delta.velocity(); - return v; - } - /// @} -}; - -// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors -template<> -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 { Pose3 pose; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp new file mode 100644 index 000000000..675fe95c0 --- /dev/null +++ b/gtsam/navigation/tests/testNavState.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 Velocity3 kVelocity(0.4, 0.5, 0.6); +static const NavState kIdentity; +static const NavState kState1(kRotation, kPosition, kVelocity); + +const double tol = 1e-5; + +/* ************************************************************************* */ +TEST( NavState, MatrixGroup ) { + // check roundtrip conversion to 7*7 matrix representation + Matrix7 T = kState1.matrix(); + EXPECT(assert_equal(kState1, NavState(T), tol)); + + // check group product agrees with matrix product + NavState state2 = kState1 * kState1; + Matrix T2 = T * T; + EXPECT(assert_equal(state2, NavState(T2), tol)); + EXPECT(assert_equal(T2, state2.matrix(), tol)); +} + +/* ************************************************************************* */ +TEST( NavState, Manifold ) { + // Check zero xi + EXPECT(assert_equal(kIdentity, kIdentity.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), kIdentity.localCoordinates(kIdentity), tol)); + EXPECT(assert_equal(kState1, kState1.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), kState1.localCoordinates(kState1), tol)); + + // Check definition of retract as operating on components separately + Vector xi(9); + 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), tol)); + EXPECT(assert_equal(xi, kState1.localCoordinates(state2), tol)); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.retract(xi); + EXPECT(assert_equal(xi, state2.localCoordinates(state3), tol)); + + // Check derivatives for ChartAtOrigin::Retract + Matrix9 aH, eH; + // For zero xi + boost::function f = boost::bind( + NavState::ChartAtOrigin::Retract, _1, boost::none); + NavState::ChartAtOrigin::Retract(zero(9), aH); + eH = numericalDerivative11(f, zero(9)); + EXPECT(assert_equal((Matrix )eH, aH)); + // For non-zero xi + NavState::ChartAtOrigin::Retract(xi, aH); + eH = numericalDerivative11(f, xi); + EXPECT(assert_equal((Matrix )eH, aH)); + + // Check retract derivatives +// Matrix9 aH1, aH2; +// kState1.retract(xi, aH1, aH2); +// Matrix eH1 = numericalDerivative11( +// boost::bind(&NavState::retract, _1, xi, boost::none, boost::none), +// kState1); +// EXPECT(assert_equal(eH1, aH1)); +// Matrix eH2 = numericalDerivative11( +// boost::bind(&NavState::retract, kState1, _1, boost::none, boost::none), +// xi); +// EXPECT(assert_equal(eH2, aH2)); +} + +/* ************************************************************************* */ +TEST( NavState, Lie ) { + // Check zero xi + EXPECT(assert_equal(kIdentity, kIdentity.expmap(zero(9)), tol)); + EXPECT(assert_equal(zero(9), kIdentity.logmap(kIdentity), tol)); + EXPECT(assert_equal(kState1, kState1.expmap(zero(9)), tol)); + EXPECT(assert_equal(zero(9), kState1.logmap(kState1), tol)); + + // 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), tol)); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.expmap(xi); + EXPECT(assert_equal(xi, state2.logmap(state3), tol)); + + // For the expmap/logmap (not necessarily expmap/local) -xi goes other way + EXPECT(assert_equal(state2, state3.expmap(-xi), tol)); + EXPECT(assert_equal(xi, -state3.logmap(state2), tol)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */