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

Abstract

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 $${\text {SE}}_{3}$$SE3 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 $${\text {SE}}_{3}$$SE3 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.