diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 93bf072c5..41f7ce810 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -33,6 +33,15 @@ namespace gtsam { //****************************************************************************** namespace so3 { +static constexpr double one_6th = 1.0 / 6.0; +static constexpr double one_12th = 1.0 / 12.0; +static constexpr double one_24th = 1.0 / 24.0; +static constexpr double one_60th = 1.0 / 60.0; +static constexpr double one_120th = 1.0 / 120.0; +static constexpr double one_180th = 1.0 / 180.0; +static constexpr double one_720th = 1.0 / 720.0; +static constexpr double one_1260th = 1.0 / 1260.0; + GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { Matrix99 H; auto R = Q.matrix(); @@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) { 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] B = one_minus_cos / theta2; } else { - // Limits as theta -> 0: - A = 1.0; - B = 0.5; + // Taylor expansion at 0 + A = 1.0 - theta2 * one_6th; + B = 0.5 - theta2 * one_24th; } } @@ -93,12 +102,12 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) E = (2.0 * B - A) / theta2; F = (3.0 * C - B) / theta2; } else { - // Limit as theta -> 0 + // Taylor expansion at 0 // TODO(Frank): flipping signs here does not trigger any tests: harden! - C = one_sixth; - D = one_twelfth; - E = one_twelfth; - F = one_sixtieth; + C = one_6th - theta2 * one_120th; + D = one_12th + theta2 * one_720th; + E = one_12th - theta2 * one_180th; + F = one_60th - theta2 * one_1260th; } } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 2845fcce7..6e2e38101 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -138,8 +138,8 @@ struct GTSAM_EXPORT ExpmapFunctor { bool nearZero; // Ethan Eade's constants: - double A; // A = sin(theta) / theta or 1 for theta->0 - double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 + double A; // A = sin(theta) / theta + double B; // B = (1 - cos(theta)) /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -159,14 +159,14 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; // Ethan's C constant used in Jacobians - double C; // (1 - A) / theta^2 or 1/6 for theta->0 + double C; // (1 - A) / theta^2 // Constant used in inverse Jacobians - double D; // (1 - A/2B) / theta2 or 1/12 for theta->0 + double D; // (1 - A/2B) / theta2 // Constants used in cross and doubleCross - double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 - double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + double E; // (2B - A) / theta2 + double F; // (3C - B) / theta2 /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -216,10 +216,6 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - - static constexpr double one_sixth = 1.0 / 6.0; - static constexpr double one_twelfth = 1.0 / 12.0; - static constexpr double one_sixtieth = 1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 3b3de6792..ce74091f0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -835,38 +835,7 @@ TEST(Pose3, Align2) { } /* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative1) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative2) { - Matrix6 actualH; - Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative3) { - Matrix6 actualH; - Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - // Small angle approximation is not as precise as numerical derivative? - EXPECT(assert_equal(expectedH, actualH, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative4) { +TEST(Pose3, ExpmapDerivative) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) @@ -900,15 +869,65 @@ TEST(Pose3, ExpmapDerivative4) { } } -/* ************************************************************************* */ -TEST( Pose3, LogmapDerivative) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3 p = Pose3::Expmap(w); - EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); - Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, {}); - EXPECT(assert_equal(expectedH, actualH)); +//****************************************************************************** +namespace test_cases { +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; +std::vector large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, + {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, + {.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}}; +} // namespace test_cases + +//****************************************************************************** +TEST(Pose3, ExpmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Expmap, xi, {}); + Matrix actualH; + Pose3::Expmap(xi, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } +} + +//****************************************************************************** +// Check logmap for all small values, as we don't want wrapping. +TEST(Pose3, Logmap) { + static constexpr bool nearZero = true; + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5)); + } + } +} + +//****************************************************************************** +// Check logmap derivatives for all values +TEST(Pose3, LogmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Logmap, pose, {}); + Matrix actualH; + Pose3::Logmap(pose, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } } /* ************************************************************************* */