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.

翻译：捆绑调整（Bundle Adjustment，BA）指同时确定传感器位姿与场景几何结构的问题，是机器人视觉中的基础问题。本文提出一种面向激光雷达传感器的高效且一致的捆绑调整方法。该方法采用边缘与平面特征表示场景几何结构，并直接最小化每个原始点到对应几何特征的自然欧氏距离。该公式化的一个优良特性在于几何特征可通过解析求解获得，从而大幅降低数值优化的维度。为更高效地表示并求解所得优化问题，本文进一步提出一种新颖的“点簇”概念，通过一组紧凑的参数——“点簇坐标”——对关联于同一特征的所有原始点进行编码。我们推导了基于点簇坐标的BA优化问题直至二阶的闭式导数，并阐明了其理论性质，如零空间与稀疏性。基于这些理论结果，本文开发了一种高效二阶BA求解器。该求解器除估计激光雷达位姿外，还利用二阶信息估计由测量噪声引起的位姿不确定性，从而获得一致的激光雷达位姿估计。此外，得益于点簇的运用，所开发的求解器从根本上避免了在优化的所有步骤（包括代价函数计算、导数计算与不确定性评估）中对海量原始点进行逐点枚举（因其数量庞大而极为耗时）。本方法的实现已开源，以惠及机器人学界及相关领域。