diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5f0e8e3ce..16dda3d1e 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -11,13 +11,14 @@ /** * @file Pose3.cpp - * @brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ -#include -#include -#include #include +#include +#include +#include +#include #include #include @@ -111,7 +112,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -158,26 +159,28 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); + interpolate(t_, T.t_, t)); } /* ************************************************************************* */ -/** Modified from Murray94book version (which assumes w and v normalized?) */ +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); // Compute rotation using Expmap - Matrix3 Jw; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - // Compute translation and optionally its Jacobian in w + // Compute translation and optionally its Jacobian Q in w + // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. Matrix3 Q; - const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); if (Hxi) { - *Hxi << Jw, Z_3x3, - Q, Jw; + *Hxi << Jr, Z_3x3, // + Q, Jr; } return Pose3(R, t); @@ -238,60 +241,50 @@ 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) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - return pose3::ExpmapFunctor(w).computeQ(v); + Matrix3 Q; + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); + return Q; } /* ************************************************************************* */ +// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas +// t_parallel = w * w.dot(v); // translation parallel to axis +// w_cross_v = w.cross(v); // translation orthogonal to axis +// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; +// but functor does not need R, deals automatically with the case where theta2 +// is near zero, and also gives us the machinery for the Jacobians. Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q, - const std::optional& R, + OptionalJacobian<3, 3> J, double nearZeroThreshold) { const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); - if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); - 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)); - Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; - return t; + // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you + // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + Matrix3 H; + Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); + + // We return Jacobians for use in Expmap, so we multiply with X, that + // translates from left to right for our right expmap convention: + if (Q) { + Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + *Q = X * H; } + + if (J) { + *J = local.rightJacobian(); // = X * local.leftJacobian(); + } + + return t; } /* ************************************************************************* */ @@ -320,7 +313,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { } /* ************************************************************************* */ - const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { if (Hself) { *Hself << I_3x3, Z_3x3; @@ -338,14 +330,14 @@ Matrix4 Pose3::matrix() const { /* ************************************************************************* */ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HaTb) const { + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HwTb) const { + OptionalJacobian<6, 6> HwTb) const { if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; @@ -354,7 +346,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, /* ************************************************************************* */ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); @@ -378,7 +370,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { /* ************************************************************************* */ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -484,7 +476,7 @@ std::optional Pose3::Align(const Point3Pairs &abPointPairs) { std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( - "Pose3:Align expects 3*N matrices of equal shape."); + "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; for (Eigen::Index j = 0; j < a.cols(); j++) { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c80..81a9848e2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -11,7 +11,7 @@ /** *@file Pose3.h - *@brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ // \callgraph @@ -223,17 +223,19 @@ public: const Vector6& xi, double nearZeroThreshold = 1e-5); /** - * Compute the translation part of the exponential map, with derivative. + * Compute the translation part of the exponential map, with Jacobians. * @param w 3D angular velocity * @param v 3D velocity * @param Q Optionally, compute 3x3 Jacobian wrpt w - * @param R Optionally, precomputed as Rot3::Expmap(w) + * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) * @param nearZeroThreshold threshold for small values - * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + * @note This function returns Jacobians Q and J corresponding to the bottom + * blocks of the SE(3) exponential, and translated from left to right from the + * applyLeftJacobian Jacobians. */ static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q = {}, - const std::optional& R = {}, + OptionalJacobian<3, 3> J = {}, double nearZeroThreshold = 1e-5); using LieGroup::inverse; // version with derivative diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index c54306ae5..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; } } @@ -87,19 +96,29 @@ 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 = nearZero ? one_sixth : (1 - A) / theta2; - D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; - E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; + if (!nearZero) { + C = (1 - A) / theta2; + D = (1.0 - A / (2.0 * B)) / theta2; + E = (2.0 * B - A) / theta2; + F = (3.0 * C - B) / theta2; + } else { + // Taylor expansion at 0 + // TODO(Frank): flipping signs here does not trigger any tests: harden! + C = one_6th - theta2 * one_120th; + D = one_12th + theta2 * one_720th; + E = one_12th - theta2 * one_180th; + F = one_60th - theta2 * one_1260th; + } } 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); + // Apply product rule to (B Wv) + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v) + *H = - Wv * E * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -111,10 +130,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, 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; + // Apply product rule to (C WWv) + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v) + *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } @@ -132,14 +151,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = rightJacobian().inverse(); - const Vector3 c = invDexp * v; + const Matrix3 invJr = rightJacobianInverse(); + const Vector3 c = invJr * v; if (H1) { - Matrix3 D_dexpv_w; - applyDexp(c, D_dexpv_w); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_w; + Matrix3 H; + applyDexp(c, H); // get derivative H of forward mapping + *H1 = -invJr * H; } - if (H2) *H2 = invDexp; + if (H2) *H2 = invJr; return c; } @@ -154,6 +173,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, return v + BWv + CWWv; } +Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invJl = leftJacobianInverse(); + const Vector3 c = invJl * v; + if (H1) { + Matrix3 H; + applyLeftJacobian(c, H); // get derivative H of forward mapping + *H1 = -invJl * H; + } + if (H2) *H2 = invJl; + return c; +} + } // namespace so3 //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index f831aa426..163ae6623 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,14 +132,16 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map +/// Math is based on Ethan Eade's elegant Lie group document, at +/// https://www.ethaneade.org/lie.pdf. struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; 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); @@ -155,12 +157,20 @@ struct GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives +/// Math extends Ethan theme of elegant I + aW + bWW expressions. +/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83). struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; - double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + + // Ethan's C constant used in Jacobians + double C; // (1 - A) / theta^2 + + // Constant used in inverse Jacobians + double D; // (1 - A/2B) / theta2 + // 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 + 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); @@ -179,6 +189,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } + /// Inverse of right Jacobian + Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } + + // Inverse of left Jacobian + Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } + + /// Synonym for rightJacobianInverse + inline Matrix3 invDexp() const { return rightJacobianInverse(); } + /// Computes B * (omega x v). Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; @@ -197,9 +216,10 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { 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; + /// Multiplies with leftJacobianInverse(), with optional derivatives + Vector3 applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ff4f9a85..b04fb1982 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,26 +869,71 @@ TEST(Pose3, ExpmapDerivative4) { } } -TEST( Pose3, ExpmapDerivativeQr) { - Vector6 w = Vector6::Random(); - w.head<3>().normalize(); - w.head<3>() = w.head<3>() * 0.9e-2; - Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); - EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); +//****************************************************************************** +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)); + } + } + } } -/* ************************************************************************* */ -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)); +//****************************************************************************** +// 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))); + } + } +} + +//****************************************************************************** +// 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); +#ifdef GTSAM_USE_QUATERNIONS + // TODO(Frank): Figure out why quaternions are not as accurate. + // Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS. + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +#else + EXPECT(assert_equal(expectedH, actualH)); +#endif + } + } + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3ecabe84b..c6fd52161 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -303,14 +303,31 @@ 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 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}, {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, JacobianInverses) { + Matrix HR, HL; + for (bool nearZero : {true, false}) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + EXPECT(assert_equal(local.rightJacobian().inverse(), + local.rightJacobianInverse())); + EXPECT(assert_equal(local.leftJacobian().inverse(), + local.leftJacobianInverse())); + } + } +} + //****************************************************************************** TEST(SO3, CrossB) { Matrix aH1; @@ -368,6 +385,28 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyInvDexp) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + Matrix invJr = local.rightJacobianInverse(); + for (const Vector3& v : test_cases::vs) { + EXPECT( + assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invJr, aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; @@ -390,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) { } //****************************************************************************** -TEST(SO3, ApplyInvDexp) { +TEST(SO3, ApplyLeftJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - Matrix invDexp = local.dexp().inverse(); + Matrix invJl = local.leftJacobianInverse(); for (const Vector3& v : test_cases::vs) { - EXPECT(assert_equal(Vector3(invDexp * v), - local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(Vector3(invJl * v), + local.applyLeftJacobianInverse(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); + EXPECT(assert_equal(invJl, aH2)); } } } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index b9d48a3cb..bd482132e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -114,24 +114,23 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w, translational velocity v, and acceleration a - Vector3 w = xi.head<3>(); - Vector3 rho = xi.segment<3>(3); - Vector3 nu = xi.tail<3>(); + // Get angular velocity w and components rho (for t) and nu (for v) from xi + Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); // Compute rotation using Expmap - Rot3 R = Rot3::Expmap(w); + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - // Compute translations and optionally their Jacobians + // Compute translations and optionally their Jacobians Q in w + // The Jacobians with respect to rho and nu are equal to Jr Matrix3 Qt, Qv; - Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); - Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); if (Hxi) { - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - *Hxi << Jw, Z_3x3, Z_3x3, - Qt, Jw, Z_3x3, - Qv, Z_3x3, Jw; + *Hxi << Jr, Z_3x3, Z_3x3, // + Qt, Jr, Z_3x3, // + Qv, Z_3x3, Jr; } return NavState(R, t, v); @@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { Matrix3 Qt, Qv; const Rot3 R = Rot3::Expmap(w); - Pose3::ExpmapTranslation(w, rho, Qt, R); - Pose3::ExpmapTranslation(w, nu, Qv, R); + Pose3::ExpmapTranslation(w, rho, Qt); + Pose3::ExpmapTranslation(w, nu, Qv); const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw;