Simultaneous localization and mapping (SLAM) is a fundamental problem in robotics and autonomous systems. Traditional SLAM back-end optimization methods, such as Extended Kalman Filter (EKF) and graph-based nonlinear optimization, often rely on iterative procedures and local linearization, which limit their robustness and computational efficiency in large-scale and highly nonlinear environments. To address these limitations, this paper proposes a novel SLAM back-end optimization algorithm based entirely on linear equations in three-dimensional space. The method introduces a virtual coordinate system for each node and formulates pose transformations as a system of linear constraints using the geometric similarity of spatial triangles and tetrahedra. By leveraging the gravity direction measured by the IMU, the algorithm decouples the horizontal and vertical components of motion, enabling efficient projection of spatial relationships onto the ground plane. Furthermore, the proposed algorithm supports various sensors, including LiDAR, IMU, and GPS, and incorporates a robust outlier removal mechanism. Extensive numerical experiments demonstrate that the proposed algorithm achieves accuracy comparable to state-of-the-art nonlinear optimization methods while significantly reducing computation time, offering a scalable and robust solution for real-time SLAM applications.
Zhitao Wu (2026) studied this question.