This repository contains different 3D SLAM solutions. The goal of this project was to compare different solutions with each other and find the best front-end LiDAR odometry system to use with LAMP, a multi-robot back-end SLAM system with pose graph optimization developed by the NeBula team.
The following front-end LiDAR odometry systems solutions were tested:
This project was only tested with Ubuntu 18.04 and ROS Melodic. No guarantee it works for other versions.
Pull the repository with all it's submodules:
git clone --recurse-submodules https://github.com/Telios/3dslam_comparison.gitInstall ROS melodic
Cmake version used: 3.27.7 See instructions for installation on Ubuntu 18.04 (Dont do step 2 !!)
See details at original repository.
Install the following libraries (could be that they are already installed, but just to be sure):
sudo apt install libpcl-dev libgoogle-glog-dev libeigen3-dev libpcl-dev libyaml-cpp-dev python-catkin-tools python-pipUpgrade your g++ and gcc compilers to 9.0:
sudo add-apt-repository ppa:ubuntu-toolchain-r/test
sudo apt update
sudo apt install gcc-9 g++-9
sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 9
sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-9 9Test if the correct version is installed:
gcc --version
g++ --versionSee here for more details about update-alternatives.
Problems with Ubuntu 18.04: Tbb library outdated, proposed solution on github repo does not work (atleast for me). Thus download the newer version of libtbb with:
- Add the Ubuntu repository that contains the latest version 2019~U8-1, run the following command in terminal:
echo "deb http://old-releases.ubuntu.com/ubuntu eoan main universe" | sudo tee -a /etc/apt/sources.list""- Update the repositories:
sudo apt update- Install the latest version of libtbb:
sudo apt install libtbb-devI recommend removing the added repository afterwards:
sudo sed -i '/old-releases/d' /etc/apt/sources.listFor more information about the tbb installation see here.
See deatils at DLO original repository and DLIO original repository
Install the following libraries (could be that they are already installed, but just to be sure):
sudo apt install libomp-dev libpcl-dev libeigen3-dev python3-catkin-toolsSee deatils at original repository.
In the root folder of the repository run:
cd LAMP/src/LAMPInstall the following pip packages in this order:
python -m pip install pytest-runner future
python -m pip install torch==1.4.0
python -m pip install -r requirements.txtTo compare the different algorithms a dataset is needed. We recorded different rosbags with an Ouster OS-128 and a Velodyne VLP-16 LiDAR. The rosbags can be downloaded from this link The datasets contain either simple loops on one floor or a multi-story building. We used the quadruped Spot from Boston Dynamics to record the data.
Make a directory in the root folder to install the datasets:
mkdir rosbagsThen copy the downloaded rosbags into the rosbags folder.
For easy building and running the system we use tmux and tmuxp. Install them with:
sudo apt install tmux tmuxpIn the folder tmuxp_configs/ you can see the different configurations for the different algorithms. To run the system with a specific configuration run:
tmuxp load tmuxp_configs/<config_name>.ymlThis will open a new tmux session with all the terminals needed to run the system.
I recommend to use an alias to stop all the processes and close the tmux session. Add the following line to your .bashrc file:
alias kt='tmux kill-session'Then you can stop the system by typing kt in the terminal. Don't forget to source ~/.bashrc after editing it. Alternatively you can also reopen the terminal.
The git submodules locus_ws contains two different branches. The master branch contains code used for datasets that are marked with velodyne. The ouster branch is used for datasets marked with ouster. To switch between the branches run:
cd locus_ws && git checkout <branch_name>You'll have to rebuild the system after switching branches.
To build the system run the following command in the 3dslam_comparison folder:
tmuxp load tmuxp_configs/build_all.ymlThis should open a new tmux session with all the terminals needed to build the system. This could take a while.
Before running the system make sure you have a results folder with in the root directory with the following structure:
results
├── pointclouds
├── profiler_resultsEvery configuration records the CPU and RAM utilization of the system. The results are stored in the profiler_results folder. The pointclouds are stored in the pointclouds folder.
To run a specific algorithm run the following command in the 3dslam_comparison folder:
tmuxp load tmuxp_configs/<config_name>.ymlThis should open a new tmux session with all the terminals needed to run the system. There are different environment variables in the yaml files that can be set to change the behaviour of the system. The following variables are available, depending on the config file:
ROBOT_NAMESPACE: The namespace of the robot.BASE_NAMESPACE: the namespace of the base station for LAMP.ROSBAG: The name of the rosbag to use.ROSBAG_FOLDER: The folder where the rosbags are stored.POINTCLOUD_TOPIC: The topic of the pointclouds in the rosbag.IMU_TOPIC: The topic of the IMU in the rosbag.RVIZ_FILE: The name of the rviz file to use.RESULTS_FOLDER: The folder where the results are stored.ODOMETRY_TOPIC: The topic of the odometry messages (used for LAMP).BAG_DURATION: The duration of the rosbag in seconds.
After you have run the system you can visualize the profiling results with the following command from the root directory:
cd results/profiler_results && python ../../scripts/plotter.pyThis script will plot and save the CPU and RAM utilization of the system. The plots are stored in the results/profiler_results folder.
To visualize the pointclouds you can use the pcd_viewer from the PCL library. Install it with:
sudo apt install pcl-toolsThen you can visualize the pointclouds with:
pcl_viewer results/pointclouds/<pointcloud_name>.pcdAlternatively and more convenient you can use the software CompareCloud
The CPU load and RAM allocation were measured with the python tool psutil. The following images show the CPU load and RAM utilization of the different algorithms. Both are shown with boxplots provided by matplotlib.
To evaluate the different algorithms we used the software CloudCompare. First a ground truth pointcloud was generated with the Riegl Laser Scanner VZ-400i
The Riegl Laser Scanner VZ-400i.
Registration of the scans.
Each pointcloud generated by the different algorithms were filtered through a statistical outlier removal filter with a mean of 6 and a standard deviation of 1.0.
Then the ground truth pointcloud was registered with the filtered pointcloud. The registration was done with the Iterative Closest Point (ICP) algorithm provided by CloudCompare. After that the C2C (cloud to cloud) distance was computed. For each point in the ground truth pointcloud the closest point in the filtered pointcloud was searched. Thus each point gets a distance value. Depening on the distance value the point is colored. The color scale is shown in the following image:
This image show the results of the g4_ouster.bag. Spot was going from the ground floor to the 4th floor and took one loop on the 4th floor. Points with distances >2 were discarded to better show the color scale.
The goal of this project was to find out what combination of front-end LiDAR odometry algorithms and LAMP will best fit with our robotics system (Spot). The PGO (pose graph optimization) of LAMP is in most cases beneficial for the overall map quality. However the computational overhead of the PGO is quite high. Thus the back-end will have to run on a different system which is connected to Spot. Only the front-end LiDAR odometry system will run on Spot.
Considering the profiling and C2C distances results, DLO and FAST-LIO are the best candidates for the front-end LiDAR odometry system. Both algorithms have a low computational overhead and produce good results. The C2C distances are quite low and the pointclouds are well aligned. Even though DLIO was performing quite well in the C2C distances, the RAM allocation was quite high. Furthermore it was not possible to pair DLIO with LAMP. Future work could be to investigate why DLIO is not working with LAMP and perhaps try to find a way to pair them.
DLO running on the g4_ouster.bag dataset.
FAST-LIO running on the g4_ouster.bag dataset.