From d0c1854e63999f0b8e394e8ae20ad4af271f22a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:04:05 -0500 Subject: [PATCH] ExpmapTranslation --- gtsam/geometry/Pose3.cpp | 120 +++++++++++++++++++++------------------ gtsam/geometry/Pose3.h | 15 +++++ 2 files changed, 80 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 894314491..15eddf7a7 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -26,8 +26,6 @@ namespace gtsam { -using std::vector; - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3) @@ -167,21 +165,23 @@ 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) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity omega and translational velocity v from twist xi + const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - // get angular velocity omega and translational velocity v from twist xi - Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); - Rot3 R = Rot3::Expmap(omega); - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis - Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; - return Pose3(R, t); - } else { - return Pose3(R, v); + // 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; } + + return Pose3(R, t); } /* ************************************************************************* */ @@ -240,55 +240,65 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, + double nearZeroThreshold) { + Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - - Matrix3 Q; - -#ifdef NUMERICAL_EXPMAP_DERIV - Matrix3 Qj = Z_3x3; - double invFac = 1.0; - Q = Z_3x3; - Matrix3 Wj = I_3x3; - for (size_t j=1; j<10; ++j) { - Qj = Qj*W + Wj*V; - invFac = -invFac/(j+1); - Q = Q + invFac*Qj; - Wj = Wj*W; - } -#else - // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(); - const Matrix3 WVW = W * V * W; - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - - 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + - 1. / 120. * (WVW * W + W * WVW); - } -#endif - + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); return Q; } +/* ************************************************************************* */ +Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q, + const std::optional& R, + double nearZeroThreshold) { + double phi2 = w.dot(w); + + if (Q) { + const Matrix3 V = skewSymmetric(v); + const Matrix3 W = skewSymmetric(w); + const Matrix3 WVW = W * V * W; + const double phi = w.norm(); + + if (std::abs(phi) > nearZeroThreshold) { + const double s = sin(phi), c = cos(phi); + const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + + // Invert the sign of odd-order terms to have the right Jacobian + *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + + (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * + ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * + (WVW * W + W * WVW); + } else { + constexpr double one_sixth = 1. / 6.; + constexpr double one_twenty_fourth = 1. / 24.; + 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); + } + } + + // TODO(Frank): this threshold is *different*. Why? + if (phi2 > std::numeric_limits::epsilon()) { + 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) / phi2; + return t; + } else { + return v; + } +} + /* ************************************************************************* */ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; - J << Jw, Z_3x3, Q, Jw; + Expmap(xi, J); return J; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23..d7c280c80 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -217,10 +217,25 @@ public: * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. + * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); + /** + * Compute the translation part of the exponential map, with derivative. + * @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 nearZeroThreshold threshold for small values + * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + */ + static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q = {}, + const std::optional& R = {}, + double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /**