Systematic Handling of Heterogeneous Geometric Primitives in Graph-SLAM Optimization

In this letter, we propose a pose-landmark graph optimization hack-end that supports maps consisting of points, lines, or planes. Our back-end allows representing both homogeneous (point-point, line-line, plane-plane) and heterogeneous measurements (point-on-line, point-on-plane, line-on-plane). Rather than treating all cases independently, we use a unified formulation that leads to both a compact derivation and a concise implementation. The additional geometric information, deriving from the use of higher dimension primitives and constraints, yields to increased robustness and widens the convergence basin of our method. We evaluate the proposed formulation both on synthetic and raw data. Finally, we made available an open-source implementation to replicate the experiments.