Simultaneous localization and mapping with active stereo vision

We present an algorithm for creating globally consistent three-dimensional maps from depth fields produced by camera-based range measurement systems. Our approach is specifically suited to dealing with the high noise levels and the large number of outliers often produced by such systems. Range data is filtered to reject outliers within each scan. The point-to-plane variant of ICP is used for local alignment, including weightings that favor nearby points and a novel outlier rejection strategy that increases the robustness for this class of data while eliminating the burden of user-specified thresholds. Global consistency is imposed on cycles by optimally distributing the cyclic discrepancy according to the local fit correlation matrices. The algorithm is demonstrated on a dataset collected by an active unstructured-light space-time stereo vision system.