Efficient and Consistent Bundle Adjustment on Lidar Point Clouds

TL;DR

This work tackles robust concurrent registration of multiple lidar scans by formulating a lidar bundle adjustment that uses edge and plane features and minimizes true point-to-feature residuals. By analytically eliminating feature parameters and introducing point clusters, the optimization reduces to pose variables only, enabling a fast, second-order solver BALM2.0 with covariance estimation for pose uncertainty. The authors prove invariances and sparsity properties, derive closed-form derivatives up to second order, and demonstrate linear scaling with the number of features and cubic scaling with the number of poses, while remaining independent of the total number of raw points. Extensive synthetic and real-world experiments show consistently higher pose and map accuracy with substantially lower computation time than prior methods, and the approach is released as open-source. The method’s accuracy, efficiency, and extensibility make it well suited for lidar-inertial fusion, multi-lidar calibration, and high-fidelity mapping tasks.

Abstract

Bundle Adjustment (BA) refers to the problem of simultaneous determination of sensor poses and scene geometry, which is a fundamental problem in robot vision. This paper presents an efficient and consistent bundle adjustment method for lidar sensors. The method employs edge and plane features to represent the scene geometry, and directly minimizes the natural Euclidean distance from each raw point to the respective geometry feature. A nice property of this formulation is that the geometry features can be analytically solved, drastically reducing the dimension of the numerical optimization. To represent and solve the resultant optimization problem more efficiently, this paper then proposes a novel concept {\it point clusters}, which encodes all raw points associated to the same feature by a compact set of parameters, the {\it point cluster coordinates}. We derive the closed-form derivatives, up to the second order, of the BA optimization based on the point cluster coordinates and show their theoretical properties such as the null spaces and sparsity. Based on these theoretical results, this paper develops an efficient second-order BA solver. Besides estimating the lidar poses, the solver also exploits the second order information to estimate the pose uncertainty caused by measurement noises, leading to consistent estimates of lidar poses. Moreover, thanks to the use of point cluster, the developed solver fundamentally avoids the enumeration of each raw point (which is very time-consuming due to the large number) in all steps of the optimization: cost evaluation, derivatives evaluation and uncertainty evaluation. The implementation of our method is open sourced to benefit the robotics community and beyond.

Figures (13)

• Figure 1: Factor graph representation of the bundle adjustment formulation.
• Figure 2: Plane and edge features used in the lidar BA. (a) The plane formulation. $\mathbf q_i$ is a point in the plane and $\mathbf n_i$ is the plane normal. (b) The line formulation. $\mathbf q_i$ is a point on the edge and $\mathbf n_i$ is the edge direction.
• Figure 3: Two operations on point cluster (a) Rigid transform (b) Cluster merging.
• Figure 4: For the $i$-the feature (either plane or edge), all points observed at the $j$-th pose are clustered as a point cluster and is represented by ${\mathbf C}_{f_{ij}}$ in its local frame.
• Figure 5: Simulation setup: A 16-channel lidar moves along a rectangular trajectory in a cuboid semi-closed space. The white line is the trajectory and the red lines are the laser points.

Theorems & Definitions (17)

• Definition 1
• Definition 2
• Theorem 1
• proof
• Theorem 2
• proof
• Remark 1
• Remark 2
• Theorem 3
• proof