Occupancy-SLAM: An Efficient and Robust Algorithm for Simultaneously Optimizing Robot Poses and Occupancy Map

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. Occupancy maps are widely used non-feature-based environment representations because they effectively classify spaces into obstacles, free, and uknown regions, providing robots with spatial information for various tasks. In this article, we propose Occupancy-SLAM, a novel optimization-based SLAM method enabling the joint optimization of robot trajectory and the occupancy map through a parameterized map representation. The 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. In our formulation, the state variables in optimization include both robot poses and occupancy values at cell vertices in the map. Moreover, a multi-resolution optimization framework utilizing occupancy maps with varying resolutions in different stages is introduced. A variation of GaussNewton method is proposed to solve the optimization problem at different stages. The proposed algorithm efficiently converges with initialization from odometry inputs. Furthermore, we propose an occupancy submap joining method within Occupancy-SLAM framework to handle large-scale problems effectively. Evaluations using simulations and practical 2D datasets demonstrate that the proposed approach can robustly obtain more accurate results than state-of-the-art techniques, with comparable computational time. Preliminary 3D results further confirm the potential of the proposed method in practical 3D applications, achieving more accurate results than existing methods.