diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7c7aa373b..b9d48a3cb 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -264,8 +264,6 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& state, //------------------------------------------------------------------------------ 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; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 8934129ee..427531415 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -490,7 +490,7 @@ TEST(NavState, Expmap_b) { (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)); + EXPECT(assert_equal(expected, p2)); } /* ************************************************************************* */ @@ -566,7 +566,7 @@ TEST(NavState, retract_localCoordinates2) { EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); - // EXPECT(assert_equal(d12, -d21)); + // NOTE(FRANK): d12 !== -d21 for arbitrary retractions. } /* ************************************************************************* */ TEST(NavState, manifold_expmap) { @@ -675,9 +675,9 @@ 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); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } }