diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8732d0aa3..f5b840d08 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -202,7 +202,15 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } /* ************************************************************************* */ -Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi) { +/** + * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix + * J(xi) = [J_(w) Z_3x3; + * Q J_(w)] + * where J_(w) is the SO3 Expmap derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is similar to formula 102 in Barfoot14tro) + */ +static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { Vector3 w(sub(xi, 0, 3)); Vector3 v(sub(xi, 3, 6)); Matrix3 V = skewSymmetric(v); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8c3c74069..39d74bd9b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,15 +204,12 @@ public: */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); -private: - static Matrix3 computeQforExpmapDerivative(const Vector6& xi); - public: - /// Left-trivialized derivative of the exponential map + /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); - /// Left-trivialized inverse derivative of the exponential map + /// Derivative of Logmap static Matrix6 LogmapDerivative(const Vector6& xi); /**