Analytical Jacobian Approximation for Direct Optimization of a Trajectory of Interpolated Poses on SE(3)

Abstract

This paper relates to time-continuous trajectory representation using direct linear interpolation on SE(3). Our approach focuses on a novel analytical Jacobian approximation of a sequence of linearly interpolated poses on SE(3). This paper shows a derivation of the proposed analytical Jacobian using retraction mapping and an approximation to the commutativity property of infinitesimal group elements. We provide plenty of evaluations for 3 different optimization problems. For the synthetic point cloud alignment problem, our proposed Jacobian is compared with a numerical one. For the synthetic pose graph optimization problem, the proposed Jacobian approximation allows us to reduce by x7 factor the state dimensions while keeping a similar magnitude of resulting error compared to the full discrete-time trajectory. Finally, we show the validity of our approach in a time-continuous approach for real-world LIDAR odometry problem.