Second Order Minimum Energy Filtering on $\operatorname{SE}_3$ with Nonlinear Measurement Equations

Abstract

Accurate camera motion estimation is a fundamental building 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 $\operatorname{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 $\operatorname{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 estimation and otherwise is on par with the state-of-the-art.