Background
The project with the full title “Learning of 3-Dimensional Maps of Unstructured Environments on a Mobile Robot” was funded by the Deutsche Forschung Gemeinschaft (DFG). It deals with the generation of 3D maps with a mobile robot in unstructured environments like search and rescue scenarios. We are now trying to carry the approach over to the underwater domain in the EU project Cooperative Cognitive Control for Autonomous Underwater Vehicles (Co3-AUVs).
The DFG project lead to a very fast and robust form of mapping approach in form of 3D Plane SLAM that has been successfully used in different application scenarios. The approach as well as results from various field experiments are presented below.
3D Plane SLAM
The mapping approach developed within this project consists of several steps
- consecutive acquisition of 3D range scans
- extraction of planes including uncertainties
- registration of scans based on plane sets
- embedding of the registrations in a pose graph
- loop detection and relaxation, i.e., 3D SLAM proper
The different steps are also illustrated in the image on the right (click to enlarge). We are using different 3D range sensors for the first step including time of flight (TOF) cameras like PMD camera and Swissranger, stereo cameras, and self-made actuated laser range finders (aLRF). Some results within the project deal with the preprocessing of these data sources, e.g., to achieve sub-pixel accurary in TOF cameras [1][2][3]. But most results are based on raw data from an aLRF in form of a SICK S300 that is rotation in a pitching motion by a simple servo. The main steps of the approach are short explained in the following.
Plane Extraction
After a scan is acquired, a set of planar-patches is extracted from the point-cloud that is associated with the related robot frame. Apart from the planar patch’s normal and distance to the origin, i.e., its defining parameters, the extraction procedure also gives their 4 x 4 covariance matrix. The plane extraction exploits the fact that a single 3D scan is based on 2.5D range measurements, the data can hence be treated as range images where an optimal fit in linear time is possible. In addition to the planes, their boundaries are extracted. The related polygons are not needed for the SLAM approach itself, the registration operates on the basic plane parameters. But the planes with their boundaries are the basis for the visualization and the actual usage of the 3D map. A detailed description of the plane extraction can be found in [4]; the formal basis and the computation of the proper uncertainties is described in [5].
Plane Based Registration
The plane based registration is the core of 3D Plane SLAM. It works by
- determining the correspondence set maximizing the global rigid body motion constraint
- finding the optimal decoupled rotations (Wahba’s problem) and translations (closed form least squares) with related uncertainties
The core of the algorithm and a formal derivation of its principles are introduced in a publication that is currently under review; please revisit this page occasionally, we will put an update here as soon as this submission is accepted. There, results in an indoor environment and a detailed comparison to ICP and NDT are also presented. The plane registration has several main advantages. First of all, it needs unlike any other approach to 3D SLAM no motion estimates from any other source like odometry, inertial navigation system, or similar. It hence still works under circumstances where all other known approaches fail. Second, it is very fast and significantly outperforms other approaches in direct comparisons with respect to computation time. As mentioned, the related detailed publication is currently under review. The experimental results presented below nevertheless already give an impression of the performance of the algorithm.
Pose Graph SLAM
The plane registration presented in the previous section is in principle already suited for generating 3D maps by merging scans together. But though the errors are relatively small, they nevertheless accumulate and are hence in principle unbounded. It is therefore of interest to use proper Simultaneous Localization and Mapping (SLAM) for which the plane registration is fortunately particularly well suited for two main reasons. First of all, its theoretical basis allows for a proper determination of the uncertainties in the registration process. Second, theoretical analysis as well as practical experiment suggest that it is particularly robust with respect to rotations. The relaxation as result of loop closing in the SLAM can hence concentrate on translational errors for which an extremely fast closed form solution is possible. A detailed description of the pose graph embedding of the plane registration and the related SLAM algorithm can also be found in a publication that is currently under review. Again, please revisit this page occasionally, an update will be provided as soon as the submission is accepted. But below are some experimental results presented that give a first impression of the performance of the approach.
Experiments and Results
3D Plane SLAM, respectively core elements of it like the plane registration have already been very successfully applied in different application scenarios. Several examples of high resolution 3D maps made with our method can be found on Youtube:
|
Bremen city center |  |
| U-boot bunker Valentin http://www.youtube.com/watch?v=Km1HhNa0Lh0 |  |
|
A search and rescue scenario |  |
Further Links to 3D Datasets and Early Experimental Results
- Disaster City, Collapsed Car Parking (full 3D Plane SLAM)
- Disaster City, Dwelling Site (full 3D Plane SLAM)
- Jacobs Lab Map (plane based registration, full 3D Plane SLAM)
- Hannover Messe, Halle 22 map (plane based registration)
- ESA Lunar Robotics Challenge (planar surface representation)
- Velodyne on Mars (planar surface representation)
References
Kaustubh Pathak, Andreas Birk, Narunas Vaskevicius, and Jann Poppinga,
Fast Registration Based on Noisy Planes with Unknown Correspondences for 3D Mapping
IEEE Transactions on Robotics, 26 (3), pp. 424 – 441, 2010
Kaustubh Pathak, Andreas Birk, Narunas Vaskevicius, Max Pfingsthorn, Soeren Schwertfeger, and Jann Poppinga
Online 3D SLAM by Registration of Large Planar Surface Segments and Closed Form Pose-Graph Relaxation
Journal of Field Robotics, Special Issue on 3D Mapping, Vol.27, Iss.1, p.52-84, Wiley, 2010
Narunas Vaskevicius, Andreas Birk, Kaustubh Pathak, and Soeren Schwertfeger
Efficient Representation in 3D Environment Modeling for Planetary Robotic Exploration
Advanced Robotics, Vol. 24, Iss. 8-9, Brill, 2010
Kaustubh Pathak, Narunas Vaskevicius and Andreas Birk
Uncertainty Analysis for Optimum Plane Extraction from Noisy 3D Range-Sensor Point-Clouds
Intelligent Service Robotics, Vol.3, Iss.1, p.37-48, Springer, 2010
Jann Poppinga, Narunas Vaskevicius, Andreas Birk, and Kaustubh Pathak
Fast Plane Detection and Polygonalization in noisy 3D Range Images
International Conference on Intelligent Robots and Systems (IROS)
IEEE Press, 2008