Second Order Minimum Energy Filtering on SE(3) with Nonlinear Measurement Equations

Accurate camera motion estimation is a fundamental build- ing block for many Computer Vision algorithms. For improved robustness, temporal consistency of translational and rotational camera velocity is often assumed by propagating motion information forward using stochastic filters. Classical stochastic filters, however, use linear approximations for the non-linear observer model and for the non-linear structure of the underlying Lie Group SE(3) and have to approximate the unknown posteriori distribution. In this paper we employ a non-linear measurement model for the camera motion estimation problem that incorporates multiple observation equations. We solve the underlying filtering problem using a novel Minimum Energy Filter on SE(3) and give explicit expressions for the optimal state variables. Experiments on the challenging KITTI benchmark show that, although a simple motion model is only employed, our approach improves rotational velocity esti- mation and otherwise is on par with the state-of-the-art.