Results on mapping underground mines with many cycles: An exercise on lazy data association
by: Sebastian, Dirk, Dave, and many others
Step 1: Obtain a map using conventional ICP scan matching. The map is globally consistent but global correspondences are false. As a result, certain pathways appear in two separate locations.
Step 2: Extract the robot path, and create a node every 5 meters. There are a total of 310 nodes in this map. Extract the relative information between any pair of adjacent nodes. Just as in SEIFs, these links are springs: they can be bent but at a penalty.
Establish data association between pairs of points. This is
done by a search for nearest neighbors, similar to (but not identical)
to the classical ICP algorithm. If two points are found to be
in correspondence, we introduce a new link that (softly) forces their location
to be equal.
Step 4: Minimize energy: we now find the configuration that
asserts the smallest deformation on all links in the resulting spring model,
thereby trading off the amount by which we have to distort the path with the amount by which our correspondences will be violated. Mathematically, this is involves matrix inversion, but it could equally be done with propagation algorithms such as loopy belief propagation.
Step 5: Erase past correspondences, and start all over again (just as in ICP). Assume that nearby nodes correspond, and introduce a spring for all such nodes. Notice that the correspondences are now quite different from the ones in the previous iteration.
Step 6: Minimize the resulting spring model once again. Some of the correspondences still aren't the right ones, which explains the geometric distortion in the model.
Step 7: Determine once again all correspondences.
Step 8: And relax again. This map is now the final fix point, as the correspondences do not change from this point on.
Step 9: Use the new path to calculate the map again. This involves a constraint version of the scan matching, in which the robot is (softly) forced onto the path generated by the spring model. The resulting map is now topologically correct.