Revamped DexpFunctor and LogmapDerivative

[dellaert]

• much needed modernization of thresholds
• added D Taylor expansion near $\pi$
• explanation of all Jacobians in new SO3.ipynb
• investigation and visualization of Taylor expansion thresholds
• wrapped so3 functors to make that possible
• refactored Logmapderivative

Deep Dive: SO(3) Jacobians

The Jacobians defined in SO3.h describe how small changes in the so(3) tangent space (represented by a 3-vector $\omega$) relate to small changes on the SO(3) manifold (represented by rotation matrices $R$). They are crucial for tasks like uncertainty propagation, state estimation (e.g., Kalman filtering), and optimization (e.g., bundle adjustment) involving rotations.

Definitions:

Let $\omega$ be a vector in the tangent space so(3), $\theta = |\omega|$ be the rotation angle, and $\Omega = \omega^\wedge$ be the 3x3 skew-symmetric matrix corresponding to $\omega$. The key coefficients used are:

• $A = \sin(\theta) / \theta$
• $B = (1 - \cos(\theta)) / \theta^2$
• $C = (1 - A) / \theta^2 = (\theta - \sin(\theta)) / \theta^3$
• $D = (1 / \theta^2) (1 - A / (2 * B))$

Note: Taylor series expansions are used for these coefficients when $\theta$ is close to zero for numerical stability.

1. Right Jacobian (rightJacobian, dexp)

• Formula: $J_r(\omega) = I - B \Omega + C \Omega^2$
• Relationship to Expmap: Relates a small change $\delta$ in the tangent space so(3) to the corresponding small rotation applied on the right on the manifold SO(3).
  $\text{Exp}(\omega + \delta) ≈ \text{Exp}(\omega) , \text{Exp}(J_r(\omega) , \delta)$
• Explanation: This Jacobian maps a perturbation $\delta$ in the tangent space (at the origin) to the equivalent perturbation $\tau = J_r(\omega) , \delta$ in the tangent space at $R = \text{Exp}(\omega)$, such that the change on the manifold corresponds to right multiplication by $\text{Exp}(\tau)$. It represents the derivative of the exponential map considering right-composition (dexp). Often associated with perturbations in the body frame.
• Synonyms: Right Jacobian of Exp, dexp, Differential of Exp (right convention).

2. Left Jacobian (leftJacobian)

• Formula: $J_l(\omega) = I + B \Omega + C \Omega^2$
• Relationship to Expmap: Relates a small change $\delta$ in the tangent space so(3) to the corresponding small rotation applied on the left on the manifold SO(3).
  $\text{Exp}(\omega + \delta) ≈ \text{Exp}(J_l(\omega) , \delta) , \text{Exp}(\omega)$
• Explanation: This Jacobian maps a perturbation $\delta$ in the tangent space (at the origin) to the equivalent perturbation $\tau = J_l(\omega) , \delta$ in the tangent space at $R = \text{Exp}(\omega)$, such that the change on the manifold corresponds to left multiplication by $\text{Exp}(\tau)$. Often associated with perturbations in the world frame. Note that $J_l(\omega) = J_r(-omega)$.
• Synonyms: Left Jacobian of Exp, Differential of Exp (left convention).

3. Inverse Right Jacobian (rightJacobianInverse, invDexp)

• Formula: $J_r^{-1}(\omega) = I + 0.5 \Omega + D \Omega^2$
• Relationship to Logmap: Relates a small rotation perturbation $\tau$ applied on the right of a rotation $R = \text{Exp}(\omega)$ back to the resulting change in the logarithm map vector (tangent space coordinates).
  $\text{Log}(R , \text{Exp}(\tau)) - \text{Log}(R) ≈ J_r^{-1}(\omega) , \tau$
• Explanation: This Jacobian maps a small rotation increment $\tau$ (applied in the body frame, i.e., right multiplication) to the corresponding change in the so(3) coordinates $\omega$. It is the derivative of the logarithm map when considering right perturbations. This is frequently needed in optimization algorithms that parameterize updates using right perturbations.
• Synonyms: Inverse Right Jacobian of Exp, invDexp, Right Jacobian of Log (often implied by context).

4. Inverse Left Jacobian (leftJacobianInverse)

• Formula: $J_l^{-1}(\omega) = I - 0.5 \Omega + D \Omega^2$
• Relationship to Logmap: Relates a small rotation perturbation $\tau$ applied on the left of a rotation $R = \text{Exp}(\omega)$ back to the resulting change in the logarithm map vector (tangent space coordinates).
  $Log(\text{Exp}(\tau) * R) - \text{Log}(R) ≈ J_l^{-1}(\omega) , \tau$
• Explanation: This Jacobian maps a small rotation increment $\tau$ (applied in the world frame, i.e., left multiplication) to the corresponding change in the so(3) coordinates $\omega$. It is the derivative of the logarithm map when considering left perturbations. Note that $J_l^{-1}(\omega) = J_r^{-1}(-\omega)$.
• Synonyms: Inverse Left Jacobian of Exp, Left Jacobian of Log (often implied by context).

[Copilot]

Pull Request Overview

The PR revamps the SO(3) Jacobian computations and associated LogmapDerivative implementations by modernizing threshold handling, renaming functions, and updating tests to reflect these changes. Key changes include:

• Renaming and refactoring functions (e.g. applyInvDexp → applyRightJacobianInverse, dexp → rightJacobian) in several modules.
• Updating DexpFunctor constructors to use explicit threshold parameters and removing nearZero flag logic.
• Revising tests in SO3 and Pose3 to validate the new API and improve numerical handling near singularities.

Reviewed Changes

Copilot reviewed 14 out of 15 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
gtsam/navigation/TangentPreintegration.cpp	Updated to use new rightJacobianInverse and rightJacobian functions instead of deprecated names.
gtsam/navigation/NavState.{h,cpp}	Added new LogmapDerivative overload and refactored DexpFunctor usage.
gtsam/geometry/tests/{testSO3.cpp, testPose3.cpp}	Updated tests to reflect API renaming and threshold-based construction in DexpFunctor.
gtsam/geometry/{SO3.h,SO3.cpp}	Modernized function names (rightJacobian, applyRightJacobian, etc.) and threshold handling.
gtsam/geometry/{Rot3Q.cpp,Rot3M.cpp,Rot3.h}	Adapted Expmap and Logmap to use revamped SO(3) implementations.
gtsam/geometry/{Pose3.h,Pose3.cpp}	Revised LogmapDerivative and ExpmapDerivative implementations to use updated functors.

Files not reviewed (1)

• gtsam/geometry/geometry.i: Language not supported

[Copilot]

Debug output using std::cout appears in the test case; these statements should be removed to keep the test output clean.

Suggested change

	std::cout << "w: " << w.transpose() << std::endl;
	for (Vector3 v : test_cases::vs) {
	std::cout << "v: " << v.transpose() << std::endl;
	for (Vector3 v : test_cases::vs) {

[dellaert]

The printouts are still there on purpose. Some windows memory issue I think, so need to resolve that via CI :-/

[dellaert]

@ProfFan I think the windows issue was resolved. I'll remove both print statements as I address other comments by you, if you have any. The biggest change is adding a Taylor expansion at pi and changing threshold values based on graphs. The remainder is refactoring and deprecating stuff.

[ProfFan]

Did a quick run down, the only concerns I have are the numerical issues near +-pi

Would be very beneficial to get an error plot as in #1938 , also relative errors plot (some of the values go zero so relative could have large relative error?)

[dellaert]

I am going to merge, but I started a new branch to fix #1233 , whose proposed fix clashes with #887 :-(
In that branch, I added the graph from #1233 in SO3.ipynb: