diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9f81204d6..6fbd2468f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -164,22 +164,22 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { } /* ************************************************************************* */ -/** Modified from Murray94book version (which assumes w and v normalized?) */ 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); @@ -251,35 +251,39 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, } /* ************************************************************************* */ +// 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); + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + + // 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) { - // Instantiate functor for Dexp-related operations: - so3::DexpFunctor local(w, nearZero); - // X translate from left to right for our right expmap convention: Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - Matrix3 H; - Vector t = local.applyLeftJacobian(v, H); *Q = X * H; - return t; - } else if (nearZero) { - // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6 - Vector3 Wv = w.cross(v); - return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth; - } else { - // NOTE(Frank): if Q is not asked we use formulas below instead of calling - // applyLeftJacobian(v), as they convey geometric insight and are faster. - 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; } + + if (J) { + *J = local.rightJacobian(); // = X * local.leftJacobian(); + } + + return t; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c80..04504de65 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -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/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index d3313b7f1..200642456 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -304,6 +304,7 @@ 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}, //, diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index b9d48a3cb..c9cd244d3 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 and nu 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;