🤖 AI Summary
Existing methods struggle to compute high-order derivatives—such as the Hessian and observed information matrix—of SE(3) objective functions efficiently and accurately, hindering rapid algorithm prototyping. This work proposes a hybrid approach that integrates analytical differentiation with vector-seed dual-number automatic differentiation, partitioning the computation at point-action interfaces to enable exact arbitrary-order derivatives from a single gradient implementation. The key innovation lies in the first-time combination of analytical Lie group Jacobians with dual-number AD, allowing exact Hessian computation via a single forward pass while effectively removing the removable singularity of SO(3)/SE(3) bases at the origin. Experiments on a canonical 6-DoF, 5-landmark problem demonstrate that the method is approximately five times faster than finite differences, achieves machine-precision accuracy, requires only about 70 additional lines of analytical code, and involves no hyperparameter tuning.
📝 Abstract
Fast prototyping of new SE(3) estimation objectives remains awkward in practice. Modern Lie-group frameworks -- GTSAM, manif, Sophus, SymForce, Ceres -- target first-order workloads through different code-generation and automatic-differentiation strategies, each optimized for a particular seam between hand-derived geometry and generic differentiation. The remaining gap is a compact, AD-safe path from these first-order primitives to exact Hessians, observed-information matrices, and higher-order derivative tensors: the quantities needed for exact Newton steps, observed-information covariance estimates, and covariance correction.
This paper presents a hybrid analytical/AD recipe for SE(3) negative log-likelihoods. The practitioner writes the NLL gradient once, generic over a scalar type, and places the analytical/AD seam at the point-action interface y = Tx. Closed-form Lie-group Jacobians are used up to this interface; AD is applied only beyond it. The same source is then instantiated with ordinary floating-point scalars for gradients, vector-seeded dual numbers for exact Hessians in a single forward-mode pass, and nested dual numbers for higher-order derivative tensors. On a representative 6-DoF, 5-landmark SE(3) NLL, the advocated seeded-Hessian path is approximately 5x faster than finite-differencing the AD gradient on this benchmark while matching a nested-AD oracle to machine precision. The implementation adds roughly 70 lines of analytical-Jacobian code over an AD-only baseline. We also identify and fix a removable singularity in the standard SO(3)/SE(3) scalar basis that would otherwise produce NaNs at the origin under seeded AD, and we audit which Lie-group derivative tensors require this stabilized basis. The result is a practical path from rapidly written SE(3) objectives to exact higher-order derivatives, with predictable runtime and no finite-difference tuning.