Second-Order Recursive Filtering on the Rigid-Motion Lie Group SE(3) Based on Nonlinear Observations

TL;DR

This paper introduces a second-order minimum energy filter on SE(3) for camera motion estimation that accurately models complex kinematics and nonlinear observations, outperforming traditional filters and matching modern visual odometry accuracy.

Contribution

It develops a second-order nonlinear filter on SE(3) that handles full geometry and nonlinear dependencies, improving motion reconstruction accuracy.

Findings

Successfully reconstructs motions in synthetic and real scenes.

Outperforms extended Kalman filters on Lie groups with linear observations.

Achieves accuracy comparable to modern visual odometry methods.

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 SE(3) and the non-linear 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 minimum energy filter with a generalized kinematic model that copes with the full geometry of SE(3) as well as with the nonlinear dependencies between the state space and observations. 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.