Eigen-Factors a Bilevel Optimization for Plane SLAM of 3D Point Clouds

TL;DR

The paper introduces Eigen-Factors, an efficient bilevel optimization algorithm for plane-based SLAM in 3D point clouds, leveraging a novel summation matrix and analytical derivatives to improve computational performance.

Contribution

It presents a new plane SLAM algorithm that uses a summation matrix for efficient error calculation and develops a methodology for derivatives on the $SE(3)$ manifold, enabling faster optimization.

Findings

EF achieves high efficiency with $O(1)$ error calculation.

The method outperforms state-of-the-art plane SLAM algorithms in synthetic and real datasets.

Analytical derivatives on $SE(3)$ facilitate faster and more accurate optimization.

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 the optimization processes (back-end) of 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.