diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ef91108eb2..79fd6b9bf8 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, p.x() * q.y() - p.y() * q.x()); } +Point3 doubleCross(const Point3 &p, const Point3 &q, // + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) { + if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose(); + if (H2) { + const Matrix3 W = skewSymmetric(p); + *H2 = W * W; + } + return gtsam::cross(p, gtsam::cross(p, q)); +} + double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { if (H1) *H1 << q.x(), q.y(), q.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 8da83817db..d5c32816eb 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {}); /// normalize, with optional Jacobian GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {}); -/// cross product @return this x q +/// cross product @return p x q GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, OptionalJacobian<3, 3> H_p = {}, OptionalJacobian<3, 3> H_q = {}); +/// double cross product @return p x (p x q) +GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}); + /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = {}, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bb1483432f..5f0e8e3ce3 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { const Vector3 w = xi.head<3>(), v = xi.tail<3>(); // Compute rotation using Expmap - Rot3 R = Rot3::Expmap(w); + Matrix3 Jw; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); // 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; } @@ -238,14 +238,37 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } +/* ************************************************************************* */ +namespace pose3 { +struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { + // Constant used in computeQ + double F; // (B - 0.5) / theta2 or -1/24 for theta->0 + + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) + : so3::DexpFunctor(omega, nearZeroApprox) { + F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; + } + + // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative + // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand + // how to compute mess below from applyLeftJacobian derivatives in w and v. + Matrix3 computeQ(const Vector3& v) const { + const Matrix3 V = skewSymmetric(v); + const Matrix3 WVW = W * V * W; + return -0.5 * V + C * (W * V + V * W - WVW) + + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); + } + + static constexpr double _one_twenty_fourth = - 1.0 / 24.0; +}; +} // namespace pose3 + /* ************************************************************************* */ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { - Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); - return Q; + return pose3::ExpmapFunctor(w).computeQ(v); } /* ************************************************************************* */ @@ -256,39 +279,13 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, 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); - } - } + if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); - // TODO(Frank): this threshold is *different*. Why? if (nearZero) { return v + 0.5 * w.cross(v); } else { + // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but + // formulas below convey geometric insight and creating functor is not free. 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)); diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2585c37be8..c54306ae5a 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -18,13 +18,14 @@ * @date December 2014 */ +#include +#include #include +#include #include #include - #include -#include #include namespace gtsam { @@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { return H; } -GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { +GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, + OptionalJacobian<9, 9> H) { Matrix3 MR = M * R.matrix(); if (H) *H = Dcompose(R); return MR; @@ -51,85 +53,107 @@ void ExpmapFunctor::init(bool nearZeroApprox) { nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - sin_theta = std::sin(theta); + const double sin_theta = std::sin(theta); + A = sin_theta / theta; const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + 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); - if (!nearZero) { - K = W / theta; - KK = K * K; - } } 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(); - K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W = K * angle; + : theta2(angle * angle), + theta(angle), + W(skewSymmetric(axis * angle)), + WW(W * W) { init(nearZeroApprox); - if (!nearZero) { - KK = K * K; - } } -SO3 ExpmapFunctor::expmap() const { - if (nearZero) - return SO3(I_3x3 + W); - else - return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); -} +SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - if (nearZero) { - dexp_ = I_3x3 - 0.5 * W; - } else { - a = one_minus_cos / theta; - b = 1.0 - sin_theta / theta; - dexp_ = I_3x3 - a * K + b * KK; + 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; +} + +Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { + // Wv = omega x v + const Vector3 Wv = gtsam::cross(omega, v); + if (H) { + // Apply product rule: + // 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; } +Vector3 DexpFunctor::doubleCrossC(const Vector3& v, + OptionalJacobian<3, 3> H) const { + // WWv = omega x (omega x * v) + Matrix3 doubleCrossJacobian; + const Vector3 WWv = + gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); + if (H) { + // Apply product rule: + // 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; +} + +// Multiplies v with left Jacobian through vector operations only. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) { - if (nearZero) { - *H1 = 0.5 * skewSymmetric(v); - } else { - // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = K * v; - const double Da = (sin_theta - 2.0 * a) / theta2; - const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * K) * skewSymmetric(v / theta); - } - } - if (H2) *H2 = dexp_; - return dexp_ * v; + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = -D_BWv_w + D_CWWv_w; + if (H2) *H2 = rightJacobian(); + return v - BWv + CWWv; } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = dexp_.inverse(); + const Matrix3 invDexp = rightJacobian().inverse(); const Vector3 c = invDexp * v; if (H1) { - Matrix3 D_dexpv_omega; - applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_omega; + Matrix3 D_dexpv_w; + applyDexp(c, D_dexpv_w); // get derivative H of forward mapping + *H1 = -invDexp * D_dexpv_w; } if (H2) *H2 = invDexp; return c; } +Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = D_BWv_w + D_CWWv_w; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; +} + } // namespace so3 //****************************************************************************** @@ -168,12 +192,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 cd67bfb8c8..f831aa4266 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -133,16 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map -class GTSAM_EXPORT ExpmapFunctor { - protected: - const double theta2; - Matrix3 W, K, KK; +struct GTSAM_EXPORT ExpmapFunctor { + const double theta2, theta; + const Matrix3 W, WW; bool nearZero; - double theta, sin_theta, one_minus_cos; // only defined if !nearZero - void init(bool nearZeroApprox = false); + // 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 - public: /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -151,34 +149,57 @@ class GTSAM_EXPORT ExpmapFunctor { /// Rodrigues formula SO3 expmap() const; + + protected: + void init(bool nearZeroApprox = false); }; /// Functor that implements Exponential map *and* its derivatives -class DexpFunctor : public ExpmapFunctor { +struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; - double a, b; - Matrix3 dexp_; + 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: /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, // Information Theory, and Lie Groups", Volume 2, 2008. - // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) - // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return dexp_; } + // Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi) + // This maps a perturbation dxi=(w,v) in the tangent space to + // a perturbation on the manifold Expmap(dexp * xi) + Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } + + // Compute the left Jacobian for Exponential map in SO(3) + Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } + + /// Differential of expmap == right Jacobian + inline Matrix3 dexp() const { return rightJacobian(); } + + /// Computes B * (omega x v). + Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives - GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, + Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + + /// Multiplies with leftJacobian(), with optional derivatives + Vector3 applyLeftJacobian(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/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index dcfb99105a..b3b751ea12 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -154,6 +154,17 @@ TEST( Point3, cross2) { } } +/* ************************************************************************* */ +TEST(Point3, doubleCross) { + Matrix aH1, aH2; + std::function f = + [](const Point3& p, const Point3& q) { return doubleCross(p, q); }; + const Point3 omega(1, 2, 3), theta(4, 5, 6); + doubleCross(omega, theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + //************************************************************************* TEST (Point3, normalize) { Matrix actualH; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index c9851f7616..4ff4f9a854 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -845,7 +845,28 @@ TEST( Pose3, ExpmapDerivative1) { } /* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative2) { +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) { // 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) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 27c143d315..3ecabe84bd 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -303,19 +303,61 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } +namespace test_cases { +std::vector small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector large{ + {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; +} // namespace test_cases + +//****************************************************************************** +TEST(SO3, CrossB) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).crossB(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { + local.crossB(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, DoubleCrossC) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).doubleCrossC(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { + local.doubleCrossC(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 (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const 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)); @@ -326,20 +368,39 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyLeftJacobian) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { + EXPECT(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.leftJacobian(), aH2)); + } + } + } +} + //****************************************************************************** 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 (const Vector3& omega : test_cases::omegas(nearZero)) { + 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 (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));