Global motion estimation from relative measurements using iterated extended Kalman filter on matrix LIE groups

In this paper, we are interested in estimating global motions (homographies, 3D rotations, 3D Euclidean motions, etc.) as well as the covariance of the estimation errors from relative measurements by exploiting the Lie group structure of the motions. We propose a generative model based on the formulation of a concentrated Gaussian distribution on matrix Lie groups. In this context, the global motion estimation problem reduces to the minimization of the sum of squared intrinsic invariant (w.r.t the right action of the Lie group on itself) errors. We derive an iterated extended Kalman filter on matrix Lie groups from the Gauss-Newton formalism on matrix Lie groups, which exhibits a low computational complexity. Experimental results on simulated data, in the context of a consistent pose registration problem, show that the proposed algorithm significantly outperforms the state of the art approaches.