diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23b..37d4735b49 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -135,10 +135,10 @@ class GTSAM_EXPORT Pose3: public LieGroup { /// @name Lie Group /// @{ - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ + /// Exponential map at identity - create a pose from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this pose static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); /** diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a936bd02a9..ca96c482f9 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -120,7 +120,7 @@ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); - if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; + if (Hp) *Hp = q.basis().transpose() * matrix().transpose() * Dp; if (HR) *HR = q.basis().transpose() * q.skew(); return q; } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3e2817752d..43bbab1ac0 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,9 +106,261 @@ 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) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + // Order is rotation, position, velocity, represented by ω,ρ,ν + Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), + nu(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(omega); + + double omega_norm = omega.norm(); + + if (omega_norm < 1e-8) { + return NavState(Rot3(), Point3(rho), Point3(nu)); + + } else { + Matrix W = skewSymmetric(omega); + double omega_norm2 = omega_norm * omega_norm; + double omega_norm3 = omega_norm2 * omega_norm; + Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + + ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); + + return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); + } +} + +//------------------------------------------------------------------------------ +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; +} + +//------------------------------------------------------------------------------ +/// The dual version of Adjoint +Vector9 NavState::AdjointTranspose(const Vector9& x, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_x) const { + const Matrix9 Ad = AdjointMap(); + const Vector9 AdTx = Ad.transpose() * x; + + // Jacobians + if (H_state) { + //TODO(Varun) + // const auto w_T_hat = skewSymmetric(AdTx.head<3>()), + // v_T_hat = skewSymmetric(AdTx.segment<3>(3)), + // a_T_hat = skewSymmetric(AdTx.tail<3>()); + // *H_state << w_T_hat, v_T_hat, // + // /* */ v_T_hat, Z_3x3; + throw std::runtime_error( + "AdjointTranpose H_state Jacobian not implemented."); + } + if (H_x) { + *H_x = Ad.transpose(); + } + + return AdTx; +} + +//------------------------------------------------------------------------------ +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; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjointTranspose(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 GTi = adjointMap(dxi).transpose(); + Hxi->col(i) = GTi * y; + } + } + const Matrix9& adT_xi = adjointMap(xi).transpose(); + if (H_y) *H_y = adT_xi; + return adT_xi * y; +} + +/* ************************************************************************* */ +Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi) > 1e-5) { + const double sinPhi = sin(phi), cosPhi = 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 + Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * P * W * W + W * W * P * W); + } else { + Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) + + 1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) + + 1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W); + } + + Q << Qv, Qp; + return Q; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw; + + return J; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Matrix3 R = state.rotation().matrix(); + + const Vector9 xi = Logmap(state); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw * Qv * Jw; + const Matrix3 Qp2 = -Jw * Qp * Jw; + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, 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 { + // return LieGroup::retract(xi, H1, H2); + Rot3 nRb = R_; Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; @@ -133,6 +388,30 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::localCoordinates(g, H1, H2); + + //TODO(Varun) Fix so that test on L680 passes + + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + + // Vector9 xi; + // 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; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + // return xi; + Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); @@ -142,15 +421,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 +493,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 f778a71231..09eea34902 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -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 @@ -46,8 +50,6 @@ class GTSAM_EXPORT NavState { dimension = 9 }; - typedef std::pair PositionAndVelocity; - /// @name Constructors /// @{ @@ -69,11 +71,14 @@ class GTSAM_EXPORT NavState { } /// 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 @@ -87,6 +92,9 @@ class GTSAM_EXPORT NavState { return Pose3(attitude(), position()); } + /// Syntactic sugar + const Rot3& rotation() const { return attitude(); }; + /// @} /// @name Derived quantities /// @{ @@ -111,9 +119,8 @@ class GTSAM_EXPORT NavState { 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 @@ -130,7 +137,26 @@ class GTSAM_EXPORT NavState { 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_); + } + + /// @} + /// @name Lie Group /// @{ // Tangent space sugar. @@ -164,6 +190,78 @@ class GTSAM_EXPORT NavState { 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; + + /// The dual version of Adjoint + Vector9 AdjointTranspose(const Vector9& x, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_x = {}) 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 = {}); + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector9 adjointTranspose(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 = {}); + }; + + /** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative + * matrix + */ + static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold = 1e-5); + /// @} /// @name Dynamics /// @{ @@ -171,8 +269,9 @@ class GTSAM_EXPORT NavState { /// 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, @@ -203,8 +302,10 @@ class GTSAM_EXPORT NavState { }; // 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 b8f0815189..a29c857f1e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -16,16 +16,21 @@ * @date July 2015 */ -#include +#include #include +#include #include - -#include +#include +#include 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,24 +41,30 @@ 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 = std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); Matrix aH1, aH2, aH3; - EXPECT( - assert_equal(kState1, - NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); - EXPECT( - assert_equal( - numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); - EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); - EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT(assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, + aH1, aH2, aH3))); + EXPECT(assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); + EXPECT(assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT(assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); } /* ************************************************************************* */ @@ -113,7 +124,7 @@ TEST( NavState, BodyVelocity) { } /* ************************************************************************* */ -TEST( NavState, Manifold ) { +TEST( NavState, Manifold) { // Check zero xi EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi))); EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity))); @@ -121,14 +132,17 @@ TEST( NavState, Manifold ) { 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); + Vector9 d; + d << 0.1, 0.1, 0.1, 0.02, 0.03, 0.04, -0.01, -0.02, -0.03; + Rot3 drot = Rot3::Expmap(d.head<3>()); + Point3 dt = Point3(d.segment<3>(3)); + Velocity3 dvel = Velocity3(d.tail<3>()); NavState state2 = NavState(kState1.attitude() * drot, kState1.position() + kState1.attitude() * dt, kState1.velocity() + kState1.attitude() * dvel); + + Vector9 xi = kState1.localCoordinates(state2); + EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -169,6 +183,182 @@ 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 pushforward, 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))); +} + +/* ************************************************************************* */ +TEST(NavState, Lie) { + 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}); + + // logmap + Matrix9 H1, H2; + auto logmap_b = NavState::Create(Rot3::Identity(), + Vector3::Zero(), Vector3::Zero()) + .localCoordinates(nav_state_b, H1, H2); + + // TODO(Varun) + Matrix6 J1, J2; + // auto logmap_pose_b = Pose3::Create(Rot3(), Vector3::Zero()) + // .localCoordinates(nav_state_b.pose(), J1, J2); + + // // Check retraction + // auto retraction_b = NavState().retract(logmap_b); + // CHECK(assert_equal(retraction_b, nav_state_b)); + + // // Test if the sum of the logmap is the same as the logmap of the product + // auto logmap_c = NavState::Create(Rot3::Identity(), + // Vector3::Zero(), Vector3::Zero()) + // .localCoordinates(nav_state_c); + + // auto logmap_bc = NavState::Create( + // gtsam::Rot3::Identity(), Eigen::Vector3d::Zero(), + // Eigen::Vector3d::Zero(), {}, {}, {}) + // .localCoordinates(nav_state_b * nav_state_c); + // Vector9 logmap_bc2 = NavState::Logmap(nav_state_b * nav_state_c); + + // Vector9 logmap_bc_sum = logmap_b + logmap_c; + // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl; + // std::cout << "logmap_bc2 = " << logmap_bc2.transpose() << std::endl; + + // // std::cout << "logmap_bc + logmap_c = " << logmap_bc_sum.transpose() << std::endl; + // // std::cout << "logmap_b + logmap_c = " << (NavState::Logmap(nav_state_b) + NavState::Logmap(nav_state_c)).transpose() << std::endl; + // // std::cout << "logmap_bc = " << logmap_bc.transpose() << std::endl; + // // CHECK(assert_equal(logmap_bc_sum, logmap_bc)); +} + /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = @@ -271,6 +461,313 @@ 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, 1e-2)); +} + +/* ************************************************************************* */ +// 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 + +/* ************************************************************************* */ +// Checks correct exponential map (Expmap) with brute force matrix exponential +TEST(NavState, Expmap_c_full) { + //TODO(Varun) + // EXPECT(assert_equal(screwNavState::expected, + // expm(screwNavState::xi), 1e-6)); + // EXPECT(assert_equal(screwNavState::expected, + // NavState::Expmap(screwNavState::xi), 1e-6)); +} + +/* ************************************************************************* */ +// 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)); +} + +/* ************************************************************************* */ +// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) +TEST(NavState, Adjoint_hat) { + //TODO(Varun) + // auto hat = [](const Vector& xi) { return ::wedge(xi); }; + // Matrix5 expected = T.matrix() * hat(screwNavState::xi) * T.matrix().inverse(); + // Matrix5 xiprime = hat(T.Adjoint(screwNavState::xi)); + + // EXPECT(assert_equal(expected, xiprime, 1e-6)); + + // Matrix5 expected2 = + // T2.matrix() * hat(screwNavState::xi) * T2.matrix().inverse(); + // Matrix5 xiprime2 = hat(T2.Adjoint(screwNavState::xi)); + // EXPECT(assert_equal(expected2, xiprime2, 1e-6)); + + // Matrix5 expected3 = + // T3.matrix() * hat(screwNavState::xi) * T3.matrix().inverse(); + + // Matrix5 xiprime3 = hat(T3.Adjoint(screwNavState::xi)); + // EXPECT(assert_equal(expected3, 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, expmaps_galore_full) { + Vector xi; + //TODO(Varun) + // NavState actual; + // xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // actual = NavState::Expmap(xi); + // EXPECT(assert_equal(expm(xi), actual, 1e-6)); + // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-6)); + + // xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9) + // .finished(); + // for (double theta = 1.0; 0.3 * theta <= M_PI; theta *= 2) { + // Vector txi = xi * theta; + // actual = NavState::Expmap(txi); + // EXPECT(assert_equal(expm(txi, 30), actual, 1e-6)); + // Vector log = NavState::Logmap(actual); + // EXPECT(assert_equal(actual, NavState::Expmap(log), 1e-6)); + // EXPECT(assert_equal(txi, log, 1e-6)); // not true once wraps + // } + + // // Works with large v as well, but expm needs 10 iterations! + // xi = + // (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished(); + // actual = NavState::Expmap(xi); + // EXPECT(assert_equal(expm(xi, 10), actual, 1e-5)); + // EXPECT(assert_equal(xi, NavState::Logmap(actual), 1e-9)); +} + +/* ************************************************************************* */ +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))); + // EXPECT(assert_equal(d12, -d21)); +} +/* ************************************************************************* */ +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() {