diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index bd2656b50..e7c2e0b55 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -101,7 +101,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { Eigen::Block v = dP(xi); Eigen::Block a = dV(xi); - // NOTE(frank): mostly copy/paste from Pose3 + // NOTE(frank): See Pose3::Expmap Rot3 nRb = Rot3::Expmap(n_omega_nb); double theta2 = n_omega_nb.dot(n_omega_nb); if (theta2 > std::numeric_limits::epsilon()) { @@ -126,7 +126,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { TIE(nRb, n_p, n_v, nTb); Vector3 n_t = n_p.vector(); - // NOTE(frank): mostly copy/paste from Pose3 + // NOTE(frank): See Pose3::Logmap Vector9 xi; Vector3 n_omega_nb = Rot3::Logmap(nRb); double theta = n_omega_nb.norm(); @@ -147,7 +147,14 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { } Matrix9 NavState::AdjointMap() const { - throw std::runtime_error("NavState::AdjointMap not implemented yet"); + // NOTE(frank): See Pose3::AdjointMap + const Matrix3 nRb = R(); + Matrix3 pAr = skewSymmetric(t()) * nRb; + Matrix3 vAr = skewSymmetric(v()) * nRb; + Matrix9 adj; + // nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV + adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb; + return adj; } Matrix7 NavState::wedge(const Vector9& xi) { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e7aab4b7c..0edac0f65 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -189,7 +189,7 @@ public: OptionalJacobian<9, 9> H = boost::none); /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms - /// it to a tangent vector at idenity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); + /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); Matrix9 AdjointMap() const; /// wedge creates Lie algebra element from tangent vector diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 675fe95c0..8cf14fec4 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -81,16 +81,16 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal((Matrix )eH, aH)); // Check retract derivatives -// Matrix9 aH1, aH2; -// kState1.retract(xi, aH1, aH2); -// Matrix eH1 = numericalDerivative11( -// boost::bind(&NavState::retract, _1, xi, boost::none, boost::none), -// kState1); -// EXPECT(assert_equal(eH1, aH1)); -// Matrix eH2 = numericalDerivative11( -// boost::bind(&NavState::retract, kState1, _1, boost::none, boost::none), -// xi); -// EXPECT(assert_equal(eH2, aH2)); + Matrix9 aH1, aH2; + kState1.retract(xi, aH1, aH2); + Matrix eH1 = numericalDerivative11( + boost::bind(&NavState::retract, _1, xi, boost::none, boost::none), + kState1); + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( + boost::bind(&NavState::retract, kState1, _1, boost::none, boost::none), + xi); + EXPECT(assert_equal(eH2, aH2)); } /* ************************************************************************* */