Efficient Construction of Globally Consistent Ladar Maps Using Pose Network Topology and Nonlinear Programming

Many instances of the mobile robot guidance mapping problem exhibit a topology that can be represented as a graph of nodes (observations) connected by edges (poses). We show that a cycle basis of this pose network can be used to generate the independent constraint equations in a natural constrained optimization formulation of the mapping problem. Explicit reasoning about the loop topology of the network can automatically generate such a cycle basis in linear time. Furthermore, in many practical cases, the pose network has sparse structure and the associated equations can then be solved in time linear in the number of images. This approach can be used to construct globally consistent maps on very large scales in very limited computation. While the technique is applicable to mapbuilding in general, and even optimization in general, it is illustrated here for batch processing of 2D ladar scans into a mobile robot guidance map.