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

TL;DR

EF introduces a plane-centric SLAM back-end that computes the entire point-to-plane error with $O(1)$ complexity via a Summation matrix $S$ and formulates a bilevel optimization where the lower level estimates plane parameters in closed form and the upper level optimizes the sensor trajectory on SE(3) with analytic gradients and Hessians. By differentiating through an accumulated matrix $Q(m{T})$ and exploiting SE(3) retractions, EF derives closed-form derivatives, enabling a second-order optimization with a structured (often block-diagonal) Hessian. Two variants are proposed: a dense, exact Hessian (EF-Dense) and an alternating approximation that yields a fast, robust, block-diagonal Hessian, improving convergence in large-scale, noisy data from RGBD and LiDAR. Extensive synthetic and real-data experiments show EF’s superior robustness and map quality, particularly under realistic noise, with code available to the community.

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.

Figures (8)

• Figure 1: Diagram of the EF. From the left, the points from a single plane are depicted observed from different poses. The algorithm then estimates the $S_t$ matrices (only once) and process the data to estimate the plane $\pi$ and its eigenvalue $\lambda$. Following we can calculate the point error, the gradient (Sec. \ref{['sec_ef_diff']}) and the Hessian (Sec. \ref{['sec_hessian']}) in two forms, in red the alternating (block diagonal) and in green the dense method. This scheme corresponds to an iterative optimization process, so after updating the poses, the processes is repeated.
• Figure 2: Evaluation results in synthetic environments with planes sweeping over such parameters as: pose perturbation, number of poses in trajectory, number of points per plane, point-to-plane noise. Top: RPE translation error in different simulator configurations, Middle: RPE rotation error in different simulator configurations, Down: Number of iterations to converge. Plotting shaded regions for the CI of 95% (default).
• Figure 3: Hessian error, percentage of the norm after injecting noise to the trajectory. This result is for 6 planes, 10 poses and 125 points/plane.
• Figure 4: Synthetic Environment. Points are generated to emulate sampling over planes. Color corresponds to different pose.
• Figure 5: Synthetic environment for 25 planes. Left: Median time per iteration. Right: total median time per optimization.

Theorems & Definitions (10)

• Definition 1
• Lemma 1
• proof
• Lemma 2
• proof
• Proposition 1
• proof
• Proposition 2
• proof
• proof