Eigen-factors a bilevel optimization for plane SLAM of 3D point clouds

Abstract

Modern depth sensors can generate a huge number of 3D points in few seconds to be later processed by Localization and Mapping algorithms. Ideally, these algorithms should handle efficiently large sizes of Point Clouds (PC) under the assumption that using more points implies more information available. The Eigen Factors (EF) is a new algorithm that solves PC 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 Summation matrix S of homogeneous points. The solution of EF is a bilevel optimization where the lower-level problem estimates the plane variables in closed-form, and the upper-level non-linear problem uses second order optimization to estimate sensor poses (trajectory). We provide a direct analytical solution for the gradient and Hessian based on the homogeneous point-plane constraint. In addition, two variants of the EF are proposed: one pure analytical derivation and a second one approximating the problem to an alternating optimization showing better convergence properties. We evaluate the optimization processes (back-end) of EF and other state-of-the-art plane SLAM algorithms in a synthetic environment, and extended to ICL dataset (RGBD) and LiDAR KITTI datasets. EF demonstrates superior robustness and accuracy of the estimated trajectory and improved map metrics. Code is publicly available at https://github.com/prime-slam/EF-plane-SLAM with python bindings and pip package.