BALM: Bundle Adjustment for Lidar Mapping

  根据点在直线或平面上的约束，构建扫面间的特征匹配，基于此使用BA算法来进一步优化LOAM等视觉里程计得到的位姿。采用了非常巧妙的方式计算直线法向量相较于位姿变化的导数。

代价函数，雅可比矩阵和海森矩阵

  在已知点和对应直线对应关系的前提下，整体代价函数表述为：
$\begin{array}{rl}\left({\mathbf{T}}^{\ast },{\mathbf{n}}^{\ast },{\mathbf{q}}^{\ast }\right)& =\arg \underset{\mathbf{T},\mathbf{n},\mathbf{q}}{\min }\frac{1}{N}\sum _{i=1}^N{\left({\mathbf{n}}^T\left({\mathbf{p}}_i-\mathbf{q}\right)\right)}^2\\ & =\arg \underset{\mathbf{T}}{\min }\left(\underset{\mathbf{n},\mathbf{q}}{\min }\frac{1}{N}\sum _{i=1}^N{\left({\mathbf{n}}^T\left({\mathbf{p}}_i-\mathbf{q}\right)\right)}^2\right)\end{array}$
  首先证明在已知点坐标的情况下，可以直接得到平面参数，将变量中的平面参数消去。同时点的位姿又和激光位姿相关，就这样用链式法则求出最终的代价函数相对于激光位姿的雅可比矩阵和海森矩阵。原文附录有推导过程的详细说明，但要注意附录第一部分$\mathbf{U}={\left[\begin{array}{lll}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}^T$这个定义多了一个转置，不然$\mathbf{\Lambda }={\mathbf{U}}^T\mathbf{A}\mathbf{U}$不可能是对角矩阵。

自适应栅格化

  从1m开始迭代划分栅格，直到一个栅格内的点属于同一个平面/直线上。如果同一个栅格内的点过多，就对点取平均。

基于BA优化的LOAM

  在LOAM得到的初始位姿的基础上，划分栅格，基于平面/直线做进一步的位姿优化。当点的数量超过阈值时，将点边缘化。