From 7cb169f12cd8b2f2e07f9334cea4a0570fcff000 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 13 Apr 2025 00:21:06 -0400 Subject: [PATCH] LogmapDerivative(xi) --- gtsam/geometry/Pose3.cpp | 82 ++++++++++++++++------------------- gtsam/geometry/Pose3.h | 17 ++------ gtsam/navigation/NavState.cpp | 9 +++- gtsam/navigation/NavState.h | 3 ++ 4 files changed, 52 insertions(+), 59 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2e0463ceb..7f0308f1f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -256,25 +256,18 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { /* ************************************************************************* */ Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { - if (Hpose) *Hpose = LogmapDerivative(pose); const Vector3 w = Rot3::Logmap(pose.rotation()); - const Vector3 T = pose.translation(); - const double t = w.norm(); - if (t < 1e-10) { - Vector6 log; - log << w, T; - return log; - } else { - const Matrix3 W = skewSymmetric(w / t); - // Formula from Agrawal06iros, equation (14) - // simplified with Mathematica, and multiplying in T to avoid matrix math - const double Tan = tan(0.5 * t); - const Vector3 WT = W * T; - const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); - Vector6 log; - log << w, u; - return log; - } + + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + + const Vector3 t = pose.translation(); + const Vector3 u = local.applyLeftJacobianInverse(t); + Vector6 xi; + xi << w, u; + if (Hpose) *Hpose = LogmapDerivative(xi); + return xi; } /* ************************************************************************* */ @@ -309,25 +302,6 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } -/* ************************************************************************* */ -Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, - double nearZeroThreshold) { - const auto w = xi.head<3>(); - const auto v = xi.tail<3>(); - - // Instantiate functor for Dexp-related operations: - bool nearZero = (w.dot(w) <= nearZeroThreshold); - so3::DexpFunctor local(w, nearZero); - - // Call applyLeftJacobian to get its Jacobian - Matrix3 H; - local.applyLeftJacobian(v, H); - - // Multiply with R^T to account for the Pose3::Create Jacobian. - const Matrix3 R = local.expmap(); - return R.transpose() * H; -} - /* ************************************************************************* */ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { Matrix6 J; @@ -335,16 +309,36 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { return J; } +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { + const Vector3 w = xi.head<3>(); + Vector3 v = xi.segment<3>(3); + + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian to get its Jacobians + Matrix3 H_t_w; + local.applyLeftJacobian(v, H_t_w); + + // Multiply with R^T to account for NavState::Create Jacobian. + const Matrix3 R = local.expmap(); + const Matrix3 Qt = R.transpose() * H_t_w; + + // Now compute the blocks of the LogmapDerivative Jacobian + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix3 Qt2 = -Jw * Qt * Jw; + + Matrix6 J; + J << Jw, Z_3x3, Qt2, Jw; + return J; +} + /* ************************************************************************* */ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { const Vector6 xi = Logmap(pose); - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Q = ComputeQforExpmapDerivative(xi); - const Matrix3 Q2 = -Jw*Q*Jw; - Matrix6 J; - J << Jw, Z_3x3, Q2, Jw; - return J; + return LogmapDerivative(xi); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 48f3253b2..baedd1c29 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -209,7 +209,10 @@ public: static Matrix6 ExpmapDerivative(const Vector6& xi); /// Derivative of Logmap - static Matrix6 LogmapDerivative(const Pose3& xi); + static Matrix6 LogmapDerivative(const Vector6& xi); + + /// Derivative of Logmap, Pose3 version. TODO(Frank): deprecate? + static Matrix6 LogmapDerivative(const Pose3& pose); // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct GTSAM_EXPORT ChartAtOrigin { @@ -217,18 +220,6 @@ public: static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; - /** - * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix - * J_r(xi) = [J_(w) Z_3x3; - * Q_r J_(w)] - * where J_(w) is the SO3 Expmap right derivative. - * (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. - */ - static Matrix3 ComputeQforExpmapDerivative( - const Vector6& xi, double nearZeroThreshold = 1e-5); - using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 50907db25..928a1dcc6 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -238,8 +238,7 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { } //------------------------------------------------------------------------------ -Matrix9 NavState::LogmapDerivative(const NavState& state) { - const Vector9 xi = Logmap(state); +Matrix9 NavState::LogmapDerivative(const Vector9& xi) { const Vector3 w = xi.head<3>(); Vector3 rho = xi.segment<3>(3); Vector3 nu = xi.tail<3>(); @@ -270,6 +269,12 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { return J; } +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Vector9 xi = Logmap(state); + return LogmapDerivative(xi); +} + //------------------------------------------------------------------------------ Matrix5 NavState::Hat(const Vector9& xi) { Matrix5 X; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 6c93a6b94..ba56fafe1 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -245,6 +245,9 @@ public: static Matrix9 ExpmapDerivative(const Vector9& xi); /// Derivative of Logmap + static Matrix9 LogmapDerivative(const Vector9& xi); + + /// Derivative of Logmap, NavState version static Matrix9 LogmapDerivative(const NavState& xi); // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP