C++ implementation of Occupancy-SLAM: An Efficient and Robust Algorithm for Simultaneously Optimizing Robot Poses and Occupancy Map. Yingyu Wang, Liang Zhao, and Shoudong Huang and Occupancy-SLAM: Simultaneously Optimizing Robot Poses and Continuous Occupancy Map. Liang Zhao, Yingyu Wang, and Shoudong Huang. In Robotics Science and Systems (RSS), 2022.
This paper considers the SLAM problem using 2D laser scans (and odometry). We propose an optimization based SLAM approach to optimize the robot trajectory and the occupancy map simultaneously. The key novelty is that the robot poses and the 2D occupancy map are optimized together, which is significantly different from existing occupancy mapping strategies where the robot poses need to be obtained first before the map can be estimated. In this formulation, the map is represented as a continuous occupancy map where each 2D point in the environment has a corresponding evidence value, and the state variables include all the robot poses and the occupancy values at the discrete grid cell nodes of the occupancy map. Based on this formulation, a multi-resolution optimization framework that uses occupancy maps with different resolutions in different stages is introduced. A variation of Gauss-Newton method is proposed to solve the optimization problem in different stages to obtain the optimized occupancy map and robot trajectory. The proposed algorithm is very efficient and can easily converge with initialization from either odometry inputs or scan matching, even when only limited key frame scans are used. Furthermore, we propose an occupancy submap joining method so that large-scale problems can be more effectively handled by integrating the submap joining method with the proposed Occupancy-SLAM. Evaluations using simulations and practical 2D laser datasets demonstrate that the proposed approach can robustly obtain more accurate robot trajectories and occupancy maps than the state-of-the-art techniques with comparable computational time.
- Eigen >= 3.4.0
- CMake >= 3.16
- OpenMP
- OpenCV
- libigl
- Intel MKL (Option)
Download Eigen 3.4.0
cd eigen-3.4.0
mkdir build
cd build
cmake ..
sudo make install
sudo apt-get install cmake
sudo apt-get install libomp-dev
git clone https://github.com/opencv/opencv.git
mkdir build
cd build
cmake ..
make
sudo make install
Download libigl
Download datasets, and place the file under the main folder.
Specify the path to libigl in CMakeLists.txt
.
Specify the path to mkl or remove all contents about mkl in CMakeLists.txt
.
git https://github.com/WANGYINGYU/Occupancy-SLAM.git
cd Occupancy-SLAM
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
make -j12
./Occupancy_SLAM
and then input
../Data/Museum/b0/Range.txt ../Data/Museum/b0/Pose.txt
Preprocess the required data into the following format and store them as txt files.
Set parameters in config.txt
. Refer to the comments in MyStruct.cpp
for the effect of each parameter.
If you find our work useful to your research, please cite the following paper:
@INPROCEEDINGS{Zhao-RSS-22,
AUTHOR = {Liang Zhao AND Yingyu Wang AND Shoudong Huang},
TITLE = {{Occupancy-SLAM: Simultaneously Optimizing Robot Poses and Continuous Occupancy Map}},
BOOKTITLE = {Proceedings of Robotics: Science and Systems},
YEAR = {2022},
ADDRESS = {New York City, NY, USA},
MONTH = {June},
DOI = {10.15607/RSS.2022.XVIII.003}
}
Our code is under MIT License.
Dataset | Initialization | Ours |
---|---|---|
ACES | ![]() |
![]() |
Intel Lab | ![]() |
![]() |
C5 | ![]() |
![]() |
Museum b0 | ![]() |
![]() |
Freiburg Building 079 | ![]() |
![]() |
Garage | ![]() |
![]() |
MIT | ![]() |
![]() |
Simu 1 | ![]() |
![]() |
Simu 2 | ![]() |
![]() |
Simu 3 | ![]() |
![]() |