留个笔记自用

BALM: Bundle Adjustment for Lidar Mapping

做什么

首先是最基础的，Structure-from-Motion（SFM），SFM可以简单翻译成运动估计，是一种基于dui8序列图片进行三维重建的算法。简单来说就是是从运动中不同角度拍摄一个物体，这样就有多张物体不同时间的照片，使用这种照片来恢复物体的三维结构。跟SLAM里的visual odometry一样，需要估计出每帧图片到世界坐标系的R,t，然后根据图片重建点云。
在这里插入图片描述
算法的关键是就是特征匹配，求两张图片之间的R,T，然后反投影得到物体的三维点，最后将多个稀疏点云融合在一起。
然后是SFM常用的一种算法Bundle Adjustment(BA)，光束调整，也就是这篇文章所做的

省去复杂的公式，BA做的也是特征匹配的工作，在不同视角的图像中选取特征点，然后将这些特征点之间进行匹配，理论上来说，在整个空间上，这些匹配点如果投影到世界坐标系应该尽可能的接近，比如两张图片中的特征点都是某个桌子的左下角，那么就算在图片上位置不同，经过R，t转回去也应该在空间上位置接近。这就是重投影误差，使用这种方式来优化R，t，同时也会优化特征点的位置。
BA往往做的是精细化建图（空间建图）的工作。

做了什么

在这里插入图片描述
传统的BA都是在视觉领域去使用的，因为如果是在点云领域，稀疏的点会很影响特征的提取，而导致特征匹配效果不佳，在这篇文章中，将BA公式从重投影误差转换成最小化从特征点到其匹配边缘或平面的距离。简单来说，视觉BA是希望重投影的空间点在距离上相近，而雷达BA则是希望投影点是在一个平面上，这种想法和激光雷达的遮挡性有关。

怎么做

在这里插入图片描述
这里的R_si和t_si表示的是第i帧scan点云的pose，p则是点云中的点，p_fi指的是第i个特征点，p_i是点在世界坐标系下的坐标（用pose得到的那个）

这张图就能看到前面的那个意思，p_fi是一个集合，表示都是一个特征（平面、边缘）的特征点（可能来自于不同的点云），q是特征平面上的点，而n是特征平面的法向量
所以说，BA的思路就像前面说的转变成了
在这里插入图片描述
第一行这里的p就是前面的那个全局点坐标（投影到世界坐标系的特征点），然后最小化他们之间的距离。就是前面所说的，利用pose投影之后，最小化在同一个世界平面或世界边缘上的点之间的距离，有用到法向量。
然后，这里将要最小化的这个距离写成了λ，表示A矩阵的第三个最大特征值，u表示的是对应的特征向量
在这里插入图片描述
上式中的p是所有投影点p的均值，A是所有点与均值点的距离矩阵，这就是平面特征的优化，这里写的感觉挺复杂的，很数学，简单来说就是这样的，对于投影到同一个平面的所有点p，计算这些点的平均点，然后计算这些点p到平均点的距离矩阵，从这个矩阵中抽几个最大值去最小化。
然后是边缘特征，这里的方式跟平面特征差不多
在这里插入图片描述

这里的所有标识都和上面一样，很好理解了，就是最小化每个特征点p的投影到边缘代表点q的距离，这里因为是边缘差异，所以构造了一个秩Tr计算平均距离。
总的来说，BA就是最小化这个最大特征值

接下来就是为了给出了优化这个的过程，首先列了两个定理
在这里插入图片描述
对于一组特征点p，它的协方差矩阵具有可解的特征向量和对应的特征值
然后是第二个定理

对于一组点和它的协方差矩阵A，它的特征值和特征向量满足上述式子。
总的来说，这里就是列了两个定理去进行关于λ的一阶导和二阶导的推导，为了后面的优化求解，公式确实很复杂，还是看后面推导的简化结果吧
有了这里的一阶导和二阶导结果，就可以把前面的λ改写
在这里插入图片描述
这其实跟传统的BA的推导流程很相似了，J是雅各比矩阵，H是海瑟矩阵
然后是再进一步变化这里的p，已知

T就是pose，这里就是把T的一阶导差值也加上
然后就可以把对点p的投影（原来是用R，t）改写成

为了简化体现，定义一个代号D为点pose关于T的一阶导矩阵
在这里插入图片描述
最后就可以将整个式子改写成

这里涉及到的数学分析有点复杂，虽然大致的意思是最小化特征点到平面和边缘的距离，但实现起来确实不容易，这篇文章有开源代码，有机会的话可以去理解一下
除了对LiDAR BA的设计外，这篇文章还提出了一个自适应体素化的方法，主要是为了解决一个问题，前面说的所有方法都需要找到特征点，并且是属于同一个平面或者边缘的特征点。简单来说，这里的方法就是，首先对所有点云用一个初始值的voxel进行体素化，如果当前voxel中的所有特征点都位于平面或者边缘上，就将voxel八分化，然后检查这8个小voxel，直到存在非特征点。
在这里插入图片描述