A Lie Algebraic Approach for Consistent Pose Registration for General Euclidean Motion

We study the problem of registering local relative pose estimates to produce a global consistent trajectory of a moving robot. Traditionally, this problem has been studied with a flat world assumption wherein the robot motion has only three degrees of freedom. In this paper, we generalize this for the full six-degrees-of-freedom Euclidean motion. Given relative pose estimates and their covariances, our formulation uses the underlying Lie algebra of the Euclidean motion to compute the absolute poses. Ours is an iterative algorithm that minimizes the sum of Mahalanobis distances by linearizing around the current estimate at each iteration. Our algorithm is fast, does not depend on a good initialization, and can be applied to large sequences in complex outdoor terrains. It can also be applied to fuse uncertain pose information from different available sources including GPS, LADAR, wheel encoders and vision sensing to obtain more accurate odometry. Experimental results using both simulated and real data support our claim