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. In IEEE Transactions on Robotics (T-RO) 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.

Joint optimization of poses and features has been extensively studied and demonstrated to yield more accurate results in feature-based SLAM problems. However, research on jointly optimizing poses and non-feature-based maps remains limited. This work 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 he key novelty lies in optimizing both robot poses and occupancy values at different cell vertices simultaneously, a significant departure from existing methods where the robot poses need to be optimized first before the map can be estimated. This paper focuses on 2D laser-based SLAM to investigate how to jointly optimize robot poses and the occupancy map. In our formulation, the state variables in optimization include all the robot poses and the occupancy values at discrete cell vertices in 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 state-of-the-art techniques with comparable computational time. Preliminary results in the 3D case further confirm the potential of the proposed method in practical 3D applications, achieving more accurate results than existing methods.

1. Eigen >= 3.4.0
2. CMake >= 3.16
3. OpenMP
4. OpenCV
5. libigl
6. 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 and put it in the project root floder or set the path in CMakeList.txt

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 -j$(nproc)

./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.

$m \times n$ matrix, where $m$ is the number of scans and $n$ is the number of beams. The range of the laser beam corresponds to each column of the matrix in the order of the corresponding angle from smallest to largest.

$m \times 3$ matrix, where $m$ is the number of poses. The three elements of each row of the matrix correspond to x, y and angle.

$m \times 3$ matrix with the same format as poses. In addition, this input must be in increments format.

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 papers:

@article{wang2025occupancy,
  author={Wang, Yingyu and Zhao, Liang and Huang, Shoudong},
  journal={IEEE Transactions on Robotics}, 
  title={Occupancy-SLAM: An Efficient and Robust Algorithm for Simultaneously Optimizing Robot Poses and Occupancy Map}, 
  year={2025},
  pages={1-20},
  doi={10.1109/TRO.2025.3578227}}

  
@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

We use one of the simulated datasets to compare the performance of different methods using high-frequency scans as inputs.

Below compares occupancy grid maps generated by Cartographer, initial guess poses used in our method with high-frequency scans (2680 scans), the result of our method using high-frequency scans (i.e., only the first stage of our method using 2680 scans), and the result of our method using low-frequency scans (268 scans).

It should be noted that although Cartographer takes 2680 scans as input, it only outputs keyframe poses, which are far fewer than the 2680 inputs, so we align its output poses to the ground truth poses using timestamps before building the occupancy grid maps. The results demonstrate that our method achieves superior outcomes in high-frequency settings and similar performance compared with low-frequency settings.