diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index d87e3164c..7faf433bb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,11 +243,15 @@ namespace pose3 { class ExpmapFunctor : public so3::DexpFunctor { private: static constexpr double one_twenty_fourth = 1.0 / 24.0; - static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; + + // Constant used in computeQ + double F; // (B - 0.5) / theta2 or -1/24 for theta->0 public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) {} + : 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 @@ -255,17 +259,8 @@ class ExpmapFunctor : public so3::DexpFunctor { Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; - - if (!nearZero) { - // Simplified from closed-form formula in Barfoot14tro eq. (102) - return -0.5 * V + C * (W * V + V * W - WVW) + - (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - - 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); - } else { - return -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); - } + return -0.5 * V + C * (W * V + V * W - WVW) + + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } }; } // namespace pose3