Merge pull request #1930 from borglab/feature/NavStateGroup
Endow NavState with group operationsrelease/4.3a0
						commit
						bb7b6b39c7
					
				|  | @ -21,13 +21,10 @@ | |||
| 
 | ||||
| #include <cmath> | ||||
| #include <iostream> | ||||
| #include <limits> | ||||
| #include <string> | ||||
| 
 | ||||
| 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<double>::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<Rot3>& 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; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Rot3>& R = {}, | ||||
|                                    double nearZeroThreshold = 1e-5); | ||||
| 
 | ||||
|   using LieGroup<Pose3, 6>::inverse; // version with derivative
 | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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<NavState, 9> { | ||||
|  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<Point3, Velocity3> 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<NavState, 9>::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<NavState> : internal::Manifold<NavState> { | ||||
| }; | ||||
| template <> | ||||
| struct traits<NavState> : public internal::LieGroup<NavState> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const NavState> : public internal::LieGroup<NavState> {}; | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -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 <gtsam/navigation/NavState.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| 
 | ||||
| #include <gtsam/base/lieProxies.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/base/testLie.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
|  | @ -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<NavState(const Rot3&, const Point3&, const Vector3&)> 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<NavState>, T2, T2); | ||||
| 
 | ||||
|   EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); | ||||
|   EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); | ||||
| 
 | ||||
|   Matrix numericalH2 = | ||||
|       numericalDerivative22(testing::compose<NavState>, 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<NavState>, T1, T2); | ||||
|   EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); | ||||
|   EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); | ||||
| 
 | ||||
|   Matrix numericalH2 = | ||||
|       numericalDerivative22(testing::compose<NavState>, 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<NavState>, 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<NavState>, 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<NavState>, T2, T3); | ||||
|   EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3)); | ||||
| 
 | ||||
|   Matrix numericalH2 = | ||||
|       numericalDerivative22(testing::between<NavState>, 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<Vector9(const NavState&, const bool&)> 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<NavState>(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<NavState>(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<NavState(const Vector9&)> f = [](const Vector9& w) { | ||||
|     return NavState::Expmap(w); | ||||
|   }; | ||||
|   Matrix expectedH = | ||||
|       numericalDerivative21<NavState, Vector9, OptionalJacobian<9, 9> >( | ||||
|           &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<Vector9(const NavState&)> f = [](const NavState& p) { | ||||
|     return NavState::Logmap(p); | ||||
|   }; | ||||
|   Matrix expectedH = | ||||
|       numericalDerivative21<Vector9, NavState, OptionalJacobian<9, 9> >( | ||||
|           &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() { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue