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

I. Introduction

The ability of a mobile robot to localize itself is critical to its autonomous operation and navigation. Consequently, there has been considerable effort on the problem of mobile robot localization and mapping. This problem is known as simultaneous localization and mapping (SLAM) and there is a vast amount of literature on this topic (see e.g., [1] for a comprehensive survey). SLAM has been especially succesful in indoor structured environments [2], [3]. For indoor mapping, the world is modeled as planar and the pose of the robot has only three degrees of freedom (2D translation and the yaw). This is known as 2D SLAM.