Eigen-Factors an Alternating Optimization for Back-end Plane SLAM of 3D Point Clouds

2023-04-03

arXiv.org

AI Summary

To address the high computational cost and low optimization efficiency of planar modeling in large-scale 3D point cloud back-end SLAM, this paper proposes a plane-based efficient optimization framework. Our method introduces three key innovations: (1) a Point Cloud Summation Matrix enabling O(1) computation of point-to-plane distance residuals; (2) an Eigen-Factors alternating optimization scheme, where analytical derivatives of eigenvalues and matrix elements are derived on the SE(3) manifold to yield a block-diagonal Hessian, significantly accelerating second-order optimization; and (3) closed-form estimation of plane parameters, reducing joint optimization to sensor trajectory only. Experiments on synthetic data, ICL-NUIM RGB-D, and KITTI LiDAR datasets demonstrate superior efficiency and robustness over state-of-the-art planar SLAM back-ends. The source code is publicly available.

Abstract

Modern depth sensors can generate a huge number of 3D points in few seconds to be latter processed by Localization and Mapping algorithms. Ideally, these algorithms should handle efficiently large sizes of Point Clouds under the assumption that using more points implies more information available. The Eigen Factors (EF) is a new algorithm that solves SLAM by using planes as the main geometric primitive. To do so, EF exhaustively calculates the error of all points at complexity $O(1)$, thanks to the {em Summation matrix} $S$ of homogeneous points. The solution of EF is highly efficient: i) the state variables are only the sensor poses -- trajectory, while the plane parameters are estimated previously in closed from and ii) EF alternating optimization uses a Newton-Raphson method by a direct analytical calculation of the gradient and the Hessian, which turns out to be a block diagonal matrix. Since we require to differentiate over eigenvalues and matrix elements, we have developed an intuitive methodology to calculate partial derivatives in the manifold of rigid body transformations $SE(3)$, which could be applied to unrelated problems that require analytical derivatives of certain complexity. We evaluate EF and other state-of-the-art plane SLAM back-end algorithms in a synthetic environment. The evaluation is extended to ICL dataset (RGBD) and LiDAR KITTI dataset. Code is publicly available at https://github.com/prime-slam/EF-plane-SLAM.

Problem

• 3D Point Cloud Processing
• Real-time Localization
• Map Construction

Innovation

• Feature Factor Algorithm
• Newton-Raphson Optimization
• Partial Derivative Computation