Efficient and Consistent Bundle Adjustment on Lidar Point Clouds

Simultaneous determination of sensor poses and scene geometry is a fundamental problem for robot vision that is often achieved by Bundle Adjustment (BA). This article presents an efficient and consistent bundle adjustment method for light detection and ranging (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 adopts and formalizes the concept of point cluster, which encodes all raw points associated to the same feature by a compact set of parameters, the 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 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.