Second-Order Recursive Filtering on the Rigid-Motion Lie Group $${\text {SE}}_{3}$$ SE 3 Based on Nonlinear Observations

Camera motion estimation from observed scene features is an important task in image processing to increase the accuracy of many methods, e.g., optical flow and structure-from-motion. Due to the curved geometry of the state space SE 3 and the nonlinear relation to the observed optical flow, many recent filtering approaches use a first-order approximation and assume a Gaussian a posteriori distribution or restrict the state to Euclidean geometry. The physical model is usually also limited to uniform motions. We propose a second-order optimal minimum energy filter that copes with the full geometry of SE 3 as well as with the nonlinear dependencies between the state space and observations., which results in a recursive description of the optimal state and the corresponding second-order operator. The derived filter enables reconstructing motions correctly for synthetic and real scenes, e.g., from the KITTI benchmark. Our experiments confirm that the derived minimum energy filter with higher-order state differential equation copes with higher-order kinematics and is also able to minimize model noise. We also show that the proposed filter is superior to state-of-the-art extended Kalman filters on Lie groups in the case of linear observations and that our method reaches the accuracy of modern visual odometry methods.