This page contains source code and datasets for our paper, Efficient Velodyne SLAM with Point and Plane Features.
In this work, we develop and test a novel simultaneous localization and mapping algorithm, which we call IC3PO (iterative closest point plus plane optimization), for Velodyne-style scanning LiDAR sensors, capable of working in both indoor and outdoor environments. The algorithm uses an efficient plane detector to rapidly provide stable features to a localization solver. When sufficiently constraining planes are unavailable, we use a unique constraint tracking algorithm to determine a minimal set of point features to provide to the solver. We test this algorithm on several indoor and outdoor datasets and analyze its performance without the aid of any additional sensors. We compare our results to state of the art algorithms, and we show superior performance on all datasets.
Please cite using:
Our algorithm is written in modern C++ (C++11 and newer) using the NRT toolkit, an in-house C++ framework for robotics, similar in spirit to ROS.
All tests as run in the paper were performed under Ubuntu 14.04. We provide no support for the code and cannot guarantee it working on any other system.
Once built, the main program (test-LiDARPlaneDetection) offers a list of available commands via the --help argument. Typical usage will look something like: ./apps/test-LiDARPlaneDetection --filename="/path/to/data.nrtlog"
The test-LiDARPlaneDetection program will launch two windows, one that shows a visualization of the current constraint matrix, and another that visualizes all data and the mapping process. Commands for the map are displayed as text on the display. Note that the program uses older nomenclature for the core IC3PO algorithm and may refer to it as PICP. The camera may be moved by holding the left mouse button to orbit, and the right mouse button to pan along the X-Y plane. If the camera angle is close to parallel with the X-Y plane, the right mouse button will instead adjust the Z position. Zooming is possible using either the scroll wheel or holding the middle mouse button.
The graph slam is powered by GTSAM. Optimization is powered by Ceres Solver. Data was recorded in NRT format and converted to MRPT, see the datasets for more information.
All source is provided on an 'as-is' basis with no warranty or support. All source code is GPL licensed, copyright the authors. Please see the source for license details.
Source code:Datasets are provided in both MRPT rawlog as well as NRT file formats. The code provided should work with both formats. The MRPT format will likely be most useful for other programs.
Data was collected using a Velodyne HDL32-E sensor mounted on a backpack. The sensor was walked through each environment at a normal walking pace in normal weather conditions. The datasets include multiple moving objects (mostly people) as they were recorded amongst higher traffic areas on the USC campus. No preprocessing has been done on the data.
Parkside (outdoor L-shaped path): nrt, mrptSee the following youtube link for video walkthroughs followed by reconstructions for each dataset.