diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 894314491..bb1483432 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -21,13 +21,10 @@ #include #include -#include #include namespace gtsam { -using std::vector; - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3) @@ -167,21 +164,23 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity omega and translational velocity v from twist xi + const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - // get angular velocity omega and translational velocity v from twist xi - Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); - Rot3 R = Rot3::Expmap(omega); - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis - Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; - return Pose3(R, t); - } else { - return Pose3(R, v); + // Compute translation and optionally its Jacobian in w + Matrix3 Q; + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); + + if (Hxi) { + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + *Hxi << Jw, Z_3x3, + Q, Jw; } + + return Pose3(R, t); } /* ************************************************************************* */ @@ -240,55 +239,68 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, + double nearZeroThreshold) { + Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - - Matrix3 Q; - -#ifdef NUMERICAL_EXPMAP_DERIV - Matrix3 Qj = Z_3x3; - double invFac = 1.0; - Q = Z_3x3; - Matrix3 Wj = I_3x3; - for (size_t j=1; j<10; ++j) { - Qj = Qj*W + Wj*V; - invFac = -invFac/(j+1); - Q = Q + invFac*Qj; - Wj = Wj*W; - } -#else - // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(); - const Matrix3 WVW = W * V * W; - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - - 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + - 1. / 120. * (WVW * W + W * WVW); - } -#endif - + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); return Q; } +/* ************************************************************************* */ +Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q, + const std::optional& R, + double nearZeroThreshold) { + const double theta2 = w.dot(w); + bool nearZero = (theta2 <= nearZeroThreshold); + + if (Q) { + const Matrix3 V = skewSymmetric(v); + const Matrix3 W = skewSymmetric(w); + const Matrix3 WVW = W * V * W; + const double theta = w.norm(); + + if (nearZero) { + static constexpr double one_sixth = 1. / 6.; + static constexpr double one_twenty_fourth = 1. / 24.; + static constexpr double one_one_hundred_twentieth = 1. / 120.; + + *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); + } else { + const double s = sin(theta), c = cos(theta); + const double theta3 = theta2 * theta, theta4 = theta3 * theta, + theta5 = theta4 * theta; + + // Invert the sign of odd-order terms to have the right Jacobian + *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + + (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * + ((1 - theta2 / 2 - c) / theta4 - + 3 * (theta - s - theta3 / 6.) / theta5) * + (WVW * W + W * WVW); + } + } + + // TODO(Frank): this threshold is *different*. Why? + if (nearZero) { + return v + 0.5 * w.cross(v); + } else { + Vector3 t_parallel = w * w.dot(v); // translation parallel to axis + Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis + Rot3 rotation = R.value_or(Rot3::Expmap(w)); + Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; + return t; + } +} + /* ************************************************************************* */ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; - J << Jw, Z_3x3, Q, Jw; + Expmap(xi, J); return J; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23..d7c280c80 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -217,10 +217,25 @@ public: * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. + * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); + /** + * Compute the translation part of the exponential map, with derivative. + * @param w 3D angular velocity + * @param v 3D velocity + * @param Q Optionally, compute 3x3 Jacobian wrpt w + * @param R Optionally, precomputed as Rot3::Expmap(w) + * @param nearZeroThreshold threshold for small values + * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + */ + static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q = {}, + const std::optional& R = {}, + double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3e2817752..b9d48a3cb 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix7 NavState::matrix() const { +Matrix5 NavState::matrix() const { Matrix3 R = this->R(); - Matrix7 T; - T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + + Matrix5 T = Matrix5::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t_; + T.block<3, 1>(0, 4) = v_; return T; } @@ -103,6 +106,161 @@ bool NavState::equals(const NavState& other, double tol) const { && equal_with_abs_tol(v_, other.v_, tol); } +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + Rot3 Rt = R_.inverse(); + return NavState(Rt, Rt * (-t_), Rt * -(v_)); +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + // Get angular velocity w, translational velocity v, and acceleration a + Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); + + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); + + // Compute translations and optionally their Jacobians + Matrix3 Qt, Qv; + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); + + if (Hxi) { + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + *Hxi << Jw, Z_3x3, Z_3x3, + Qt, Jw, Z_3x3, + Qv, Z_3x3, Jw; + } + + return NavState(R, t, v); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { + if (Hstate) *Hstate = LogmapDerivative(state); + + const Vector3 phi = Rot3::Logmap(state.rotation()); + const Vector3& p = state.position(); + const Vector3& v = state.velocity(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, p, v; + return log; + + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + Vector9 log; + // Order is ω, p, v + log << phi, rho, nu; + return log; + } +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; + Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + // Eqn 2 in Barrau20icra + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_xib) const { + const Matrix9 Ad = AdjointMap(); + + // Jacobians + if (H_state) *H_state = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + + return Ad * xi_b; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::adjointMap(const Vector9& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + + const Matrix9& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + + return ad_xi * y; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + Matrix9 J; + Expmap(xi, J); + return J; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Vector9 xi = Logmap(state); + const Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); + + Matrix3 Qt, Qv; + const Rot3 R = Rot3::Expmap(w); + Pose3::ExpmapTranslation(w, rho, Qt, R); + Pose3::ExpmapTranslation(w, nu, Qv, R); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix3 Qt2 = -Jw * Qt * Jw; + const Matrix3 Qv2 = -Jw * Qv * Jw; + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, + Qt2, Jw, Z_3x3, + Qv2, Z_3x3, Jw; + return J; +} + + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + ChartJacobian Hxi) { + return Expmap(xi, Hxi); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& state, + ChartJacobian Hstate) { + return Logmap(state, Hstate); +} + //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { @@ -142,15 +300,16 @@ Vector9 NavState::localCoordinates(const NavState& g, // Matrix3 D_xi_R; xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; if (H1) { - *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - D_dt_R, -I_3x3, Z_3x3, // - D_dv_R, Z_3x3, -I_3x3; + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; } if (H2) { - *H2 << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, dR.matrix(), Z_3x3, // - Z_3x3, Z_3x3, dR.matrix(); + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } + return xi; } @@ -213,7 +372,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 35ce435a3..95ee71fe9 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -12,7 +12,7 @@ /** * @file NavState.h * @brief Navigation state composing of attitude, position, and velocity - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 **/ @@ -25,14 +25,18 @@ namespace gtsam { /// Velocity is currently typedef'd to Vector3 -typedef Vector3 Velocity3; +using Velocity3 = Vector3; /** * Navigation state: Pose (rotation, translation) + velocity - * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold + * Following Barrau20icra, this class belongs to the Lie group SE_2(3). + * This group is also called "double direct isometries”. + * + * NOTE: While Barrau20icra follow a R,v,t order, + * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState { -private: +class GTSAM_EXPORT NavState : public LieGroup { + private: // TODO(frank): // - should we rename t_ to p_? if not, we should rename dP do dT @@ -44,7 +48,6 @@ public: inline constexpr static auto dimension = 9; - typedef std::pair PositionAndVelocity; /// @name Constructors /// @{ @@ -67,11 +70,14 @@ public: } /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, - OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, - OptionalJacobian<9, 3> H3); + OptionalJacobian<9, 3> H1 = {}, + OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 3> H3 = {}); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, - OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + OptionalJacobian<9, 6> H1 = {}, + OptionalJacobian<9, 3> H2 = {}); /// @} /// @name Component Access @@ -109,9 +115,8 @@ public: Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// 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; + /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] + Matrix5 matrix() const; /// @} /// @name Testable @@ -128,7 +133,29 @@ public: bool equals(const NavState& other, double tol = 1e-8) const; /// @} - /// @name Manifold + /// @name Group + /// @{ + + /// identity for group operation + static NavState Identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + using LieGroup::inverse; // version with derivative + + /// compose syntactic sugar + NavState operator*(const NavState& T) const { + return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); + } + + /// Syntactic sugar + const Rot3& rotation() const { return attitude(); }; + + /// @} + /// @name Lie Group /// @{ // Tangent space sugar. @@ -162,6 +189,59 @@ public: OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; + /** + * Exponential map at identity - create a NavState from canonical coordinates + * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ + */ + static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); + + /** + * Log map at identity - return the canonical coordinates \f$ + * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState + */ + static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); + + /** + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) + * frame to the world spatial frame. + */ + Matrix9 AdjointMap() const; + + /** + * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() + */ + Vector9 Adjoint(const Vector9& xi_b, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_xib = {}) const; + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] + */ + static Matrix9 adjointMap(const Vector9& xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}, + OptionalJacobian<9, 9> H_y = {}); + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi); + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const NavState& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct GTSAM_EXPORT ChartAtOrigin { + static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); + static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); + }; + /// @} /// @name Dynamics /// @{ @@ -169,8 +249,9 @@ public: /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, - const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, - OptionalJacobian<9, 3> G2) const; + const double dt, OptionalJacobian<9, 9> F = {}, + OptionalJacobian<9, 3> G1 = {}, + OptionalJacobian<9, 3> G2 = {}) const; /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, @@ -201,8 +282,10 @@ private: }; // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors -template<> -struct traits : internal::Manifold { -}; +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b8f081518..427531415 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -12,13 +12,16 @@ /** * @file testNavState.cpp * @brief Unit tests for NavState - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 */ #include -#include + +#include #include +#include +#include #include @@ -26,6 +29,9 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(NavState) +GTSAM_CONCEPT_LIE_INST(NavState) + static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); static const Pose3 kPose(kAttitude, kPosition); @@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); +static const Point3 V(3, 0.4, -2.2); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const Point3 V2(-6.5, 3.5, 6.2); +static const Point3 P2(3.5, -8.2, 4.2); +static const NavState T(R, P2, V2); +static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); +static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), + Point3(1, 2, 3)); + /* ************************************************************************* */ TEST(NavState, Constructor) { std::function create = @@ -46,14 +62,14 @@ TEST(NavState, Constructor) { assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); EXPECT( - assert_equal( - numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); +assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); } /* ************************************************************************* */ @@ -64,7 +80,7 @@ TEST(NavState, Constructor2) { Matrix aH1, aH2; EXPECT( assert_equal(kState1, - NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); + NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); } @@ -127,8 +143,8 @@ TEST( NavState, Manifold ) { Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); NavState state2 = NavState(kState1.attitude() * drot, - kState1.position() + kState1.attitude() * dt, - kState1.velocity() + kState1.attitude() * dvel); + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -169,6 +185,143 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Equals) { + NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3)); + NavState pose2 = T3; + EXPECT(T3.equals(pose2)); + NavState origin; + EXPECT(!T3.equals(origin)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; + auto a_bc = nav_state_a * (nav_state_b * nav_state_c); + CHECK(assert_equal(ab_c, a_bc)); + + Matrix actual = (T2 * T2).matrix(); + + Matrix expected = T2.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its push-forward, another case +TEST(NavState, Compose2) { + const NavState& T1 = T; + Matrix actual = (T1 * T2).matrix(); + Matrix expected = T1.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Inverse) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto a_inv = nav_state_a.inverse(); + auto a_a_inv = nav_state_a * a_inv; + CHECK(assert_equal(a_a_inv, NavState())); + + auto b_inv = nav_state_b.inverse(); + auto b_b_inv = nav_state_b * b_inv; + CHECK(assert_equal(b_b_inv, NavState())); + + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, InverseDerivatives) { + Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5); + Vector3 v(3.5, -8.2, 4.2); + Point3 p(3.5, -8.2, 4.2); + NavState T(R, p, v); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose_Inverse) { + NavState actual = T * T.inverse(); + NavState expected; + EXPECT(assert_equal(actual, expected, 1e-8)); +} + +/* ************************************************************************* */ +TEST(NavState, Between) { + NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); + + NavState actual = s1.compose(s2); + EXPECT(assert_equal(s2, actual)); + + NavState between = s2.between(s1); + NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); + EXPECT(assert_equal(expected_between, between)); + + NavState expected = T2.inverse() * T3; + Matrix actualDBetween1, actualDBetween2; + actual = T2.between(T3, actualDBetween1, actualDBetween2); + EXPECT(assert_equal(expected, actual)); + + Matrix numericalH1 = + numericalDerivative21(testing::between, T2, T3); + EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::between, T2, T3); + EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0))); +} + /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = @@ -189,7 +342,7 @@ TEST(NavState, Coriolis) { 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)); + Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); // first-order state2.coriolis(dt, kOmegaCoriolis, false, aH); @@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless planet with an attached nav frame at - * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + /** Consider a massless planet with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously - * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and * second order Coriolis corrections are as expected. */ @@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) { bRn = nRb.inverse(); // Get expected first and second order corrections in the nav frame - Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), - n_dV1e = dt * n_aCorr1, + n_dV1e = dt * n_aCorr1, n_dV2e = dt * (n_aCorr1 + n_aCorr2); // Get expected first and second order corrections in the body frame @@ -271,6 +424,262 @@ TEST(NavState, Stream) EXPECT(os.str() == expected); } +/* ************************************************************************* */ +TEST(NavState, Print) { + NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); + + // Generate the expected output + std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + std::string p = "p: 1 2 3\n"; + std::string v = "v: 1 2 3\n"; + std::string expected = R + p + v; + + EXPECT(assert_print_equal(expected, state)); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST(NavState, Retract_first_order) { + NavState id; + Vector v = Z_9x1; + v(0) = 0.3; + EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), + id.retract(v), 1e-2)); + v(3) = 0.2; + v(4) = 0.7; + v(5) = -2; + v(6) = 3; + v(7) = 0.4; + v(8) = -2.2; + EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2)); +} +#endif + +/* ************************************************************************* */ +TEST(NavState, RetractExpmap) { + Vector xi = Z_9x1; + xi(0) = 0.3; + NavState pose = NavState::Expmap(xi), + expected(R, Point3(0, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expected, pose, 1e-2)); + EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_A_Full) { + NavState id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), + NavState(R, Point3(0, 0, 0), Point3(0, 0, 0)))); + xi(3) = -0.2; + xi(4) = -0.394742; + xi(5) = 2.08998; + xi(6) = 0.2; + xi(7) = 0.394742; + xi(8) = -2.08998; + + NavState expected(R, -P, P); + EXPECT(assert_equal(expected, expmap_default(id, xi), 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_b) { + NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0)); + NavState p2 = p1.retract( + (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), + Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwNavState { +double a = 0.3, c = cos(a), s = sin(a), w = 0.3; +Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); +Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); +Point3 expectedV(0.29552, 0.0446635, 1); +Point3 expectedP(0.29552, 0.0446635, 1); +NavState expected(expectedR, expectedV, expectedP); +} // namespace screwNavState + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(NavState, Adjoint_full) { + NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse(); + Vector xiPrime = T.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6)); + + NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse(); + Vector xiPrime2 = T2.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6)); + + NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse(); + Vector xiPrime3 = T3.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Adjoint_compose_full) { + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const NavState& T1 = T; + Vector x = + (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + NavState expected = T1 * NavState::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + NavState actual = T1 * T2 * NavState::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Retract_LocalCoordinates) { + Vector9 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d /= 10; + const Rot3 R = Rot3::Retract(d.head<3>()); + NavState t = NavState::Retract(d); + EXPECT(assert_equal(d, NavState::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates) { + Vector9 d12; + d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d12 /= 10; + NavState t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(NavState, expmap_logmap) { + Vector d12 = Vector9::Constant(0.1); + NavState t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates2) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + // NOTE(FRANK): d12 !== -d21 for arbitrary retractions. +} +/* ************************************************************************* */ +TEST(NavState, manifold_expmap) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12, -d21)); +} + +/* ************************************************************************* */ +TEST(NavState, subgroups) { + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = + (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + NavState T2 = NavState::Expmap(2 * d); + NavState T3 = NavState::Expmap(3 * d); + NavState T5 = NavState::Expmap(5 * d); + EXPECT(assert_equal(T5, T2 * T3)); + EXPECT(assert_equal(T5, T3 * T2)); +} + +/* ************************************************************************* */ +TEST(NavState, adjointMap) { + Matrix res = NavState::adjointMap(screwNavState::xi); + Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1), + screwNavState::xi(2)); + Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4), + screwNavState::xi(5)); + Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7), + screwNavState::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected, res, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState::Expmap(w, actualH); + + std::function f = [](const Vector9& w) { + return NavState::Expmap(w); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST(NavState, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState p = NavState::Expmap(w); + EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5)); + + std::function f = [](const NavState& p) { + return NavState::Logmap(p); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Logmap, p, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +//****************************************************************************** +TEST(NavState, Invariants) { + NavState id; + + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T3)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T3)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T3)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T3)); +} + +//****************************************************************************** +TEST(NavState, LieGroupDerivatives) { + NavState id; + + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T3); +} + +//****************************************************************************** +TEST(NavState, ChartDerivatives) { + NavState id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); + } +} /* ************************************************************************* */ int main() {