LogmapDerivative(xi)

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);
 }
 
 /* ************************************************************************* */

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<Pose3, 6>::inverse; // version with derivative
 
   /**

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;

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