From e96d8487e493d9cce4f60a09ab7ec20c2412210e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:54:30 -0500 Subject: [PATCH] New constants for cross and doubleCross --- gtsam/geometry/SO3.cpp | 65 ++++++++++++------------ gtsam/geometry/SO3.h | 22 ++++++--- gtsam/geometry/tests/testSO3.cpp | 84 +++++++++++++++++++++++--------- 3 files changed, 108 insertions(+), 63 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 13600f884..e341911b9 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, } void ExpmapFunctor::init(bool nearZeroApprox) { - WW = W * W; nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - sin_theta = std::sin(theta); - const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + const double sin_theta = std::sin(theta); A = sin_theta / theta; + const double s2 = std::sin(theta / 2.0); + const double one_minus_cos = + 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; } } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) { - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + : theta2(omega.dot(omega)), + theta(std::sqrt(theta2)), + W(skewSymmetric(omega)), + WW(W * W) { init(nearZeroApprox); } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) - : theta2(angle * angle), theta(angle) { - const double ax = axis.x(), ay = axis.y(), az = axis.z(); - W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W *= angle; + : theta2(angle * angle), + theta(angle), + W(skewSymmetric(axis * angle)), + WW(W * W) { init(nearZeroApprox); } -SO3 ExpmapFunctor::expmap() const { - if (nearZero) - return SO3(I_3x3 + W + 0.5 * WW); - else - return SO3(I_3x3 + A * W + B * WW); -} +SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - C = (1 - A) / theta2; + C = nearZero ? one_sixth : (1 - A) / theta2; + D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; + E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; } Matrix3 DexpFunctor::rightJacobian() const { if (nearZero) { - return I_3x3 - 0.5 * W; // + one_sixth * WW; + return I_3x3 - B * W; // + C * WW; } else { return I_3x3 - B * W + C * WW; } @@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x * v const Vector3 Wv = gtsam::cross(omega, v); if (H) { - // 1x3 Jacobian of B with respect to omega - const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); // Apply product rule: - *H = Wv * HB - B * skewSymmetric(v); + // D * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) + *H = Wv * D * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, const Vector3 WWv = gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); if (H) { - // 1x3 Jacobian of C with respect to omega - const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); // Apply product rule: - *H = WWv * HC + C * doubleCrossJacobian; + // E * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) + *H = WWv * E * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } @@ -134,12 +136,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, if (H2) *H2 = I_3x3 - 0.5 * W; return v - 0.5 * gtsam::cross(omega, v); } else { - Matrix3 D_BWv_omega, D_CWWv_omega; + Matrix3 D_BWv_omega, D_CWWv_omega; const Vector3 BWv = cross(v, D_BWv_omega); const Vector3 CWWv = doubleCross(v, D_CWWv_omega); if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; - if (H2) *H2 = rightJacobian(); - return v - BWv + CWWv; + if (H2) *H2 = rightJacobian(); + return v - BWv + CWWv; } } @@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector& rotations) { template <> GTSAM_EXPORT Matrix3 SO3::Hat(const Vector3& xi) { - // skew symmetric matrix X = xi^ - Matrix3 Y = Z_3x3; - Y(0, 1) = -xi(2); - Y(0, 2) = +xi(1); - Y(1, 2) = -xi(0); - return Y - Y.transpose(); + return skewSymmetric(xi); } //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 8766fecbe..6403c3ae0 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); /// Functor implementing Exponential map class GTSAM_EXPORT ExpmapFunctor { protected: - const double theta2; - Matrix3 W, WW; - double A, B; // Ethan Eade's constants + const double theta2, theta; + const Matrix3 W, WW; bool nearZero; - double theta, sin_theta, one_minus_cos; // only defined if !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 void init(bool nearZeroApprox = false); @@ -156,17 +157,19 @@ class GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives class DexpFunctor : public ExpmapFunctor { protected: - static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; - double C; // Ethan Eade's C constant + double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + // Constants used in cross and doubleCross + double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 + double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + public: /// Computes B * (omega x v). Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; /// Computes C * (omega x (omega x v)). Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - public: /// Constructor with element of Lie algebra so(3) GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor { GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + + protected: + 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/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 1e105ceca..e8de2e93d 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } +namespace test_cases { +std::vector nearZeros{ + {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector omegas{ + {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; +} // namespace test_cases + +//****************************************************************************** +TEST(SO3, Cross) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).cross(v); + }; + const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; + for (Vector3 omega : omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { + local.cross(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, DoubleCross) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).doubleCross(v); + }; + const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; + for (Vector3 omega : omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { + local.doubleCross(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + return so3::DexpFunctor(omega, nearZero).applyDexp(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.dexp() * v), local.applyDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); @@ -329,18 +373,16 @@ TEST(SO3, ApplyDexp) { //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; - for (bool nearZeroApprox : {false, true}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { CHECK(assert_equal(Vector3(local.leftJacobian() * v), - local.applyLeftJacobian(v, aH1, aH2))); + local.applyLeftJacobian(v, aH1, aH2))); CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); CHECK(assert_equal(local.leftJacobian(), aH2)); @@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) { //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 v : test_cases::vs) { EXPECT(assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));