本文详细探讨了激光雷达与图像光束法在误差方程构建上的差异，重点介绍了利用特征值优化点到线和点到面距离的方法，以及如何通过二阶求导计算位姿变化对特征值的影响。文章展示了特征值求导和泰勒展开在BA优化中的应用。
这篇文档推导起来需要个一整天时间，懵逼一上午，推导一下午。
目录
一. 前言
1） 激光雷达的光束法平差和 图像的光束法平差对比
差异是，图像使用投影误差来构建误差方程，优化帧位姿和地图点位置。
激光雷达则是使用点到线和点到面的距离来构建误差方程，只优化当前帧位姿。
2）点到线和点到面的距离构建
3）三个贡献
(1)相比于图像光束法，激光雷达的光束法只优化位姿，优化参数的维度很小。使快速大尺度稠密特征点优化成为可能。（因为只优化位姿）。
（2） 为确保高效BA，构建了二阶海森矩阵来求解。
(3) 提出了一个自适应体素搜索来进行高效的特征关联。
（4） 整合进loam，在固定式激光雷达和旋转激光雷达上做了测试，证明了取得的效果，在一个滑动窗口（20帧）内，做局部的BA，可以达到10HZ.
二. 方法
1) 点到线、点到面误差方程构建--->转到特征值上
把点变换到全局中：
构建的点到面的误差方程：
其中认为q的最优值为pi的平均坐标，lamaba3为最小特征值，n的最优值为对应的特征向量，此时n相当于是平面法向。
构建的点到线的误差方程:
其中认为q的最优值为pi的平均坐标，lamaba1为最大特征值，n的最优值为对应的特征向量，此时n相当于是三维线的方向。Tr(A)为A的三个对角线元素的和，也等于三个特征值的和，因此Tr(A)-lamaba1 = lamaba2+ lamaba3.
从公式3和公式4来看，q点移动并不影响最终的点到面的距离的长度 和 点到线的距离的长度，因此q的值是不唯一的，但是这不影响优化参数。 因此激光雷达的BA优化方程变成了最小化矩阵A的特征值:
2) 特征值对点pi的一阶求导
具体推导为：
这个公式不难理解，公式(19）的第一行其实可以展开成(pi-p)*uk*uk*(pi - p拔) + (pj-p)*uk*uk*(pj- p拔)两个相加项分别对pi进行求导就得到公式(19)中中的第3行和第四行，相加后由于所有pi相加就等于p拔，所以相加后就只剩下公式(19)中的最后一行了。这一部分很好理解。这一部分就是一阶导了。
3）appendix A中的公式推导
（1）由于 A* U = LAMABA* U, LAMABA为三个特征值所构建的对角矩阵，所以 UT* A* U = LAMABA
4）特征值对P点的求导,计算c(m,n)
注意后续P点会对旋转和平移求导，这里先对P点进行求导。三个乘数项分别对p进行求导得到：
根据公式（15），可以得到：
所以 公式（16)就变成了:
（1) 特征向量矩阵U对P的求导，
U对特征值对应的特征向量所形成的矩阵，因此是一个单位正交矩阵，则：
公式两边都对P进行求导得到：
简写成：
在这里很清楚的要记住一个特点，Cq + CqT = 0 ，则Cq是一个反对称矩阵，则Cq的对角线元素为0。
（4）综合公式(18)和3）中的内容，，可以得到特征值对q的求导为：
（2) 计算C(m,n)的值
根据公式(20), 特征值对角阵C 的非对角元素为0， 非对角元素对q的求导也为0，因此下式左边为0，右边改成具体的(m,n)意思是m行n列。um是第m个特征值对应的特征向量，un是第n个特征值对应的特征向量。
直接移项，因此C(m,n)为：
其中 C是反对称矩阵，因此在对角线上是0.
5）计算uk对p的求导
uk = U * e ,在第一个特征值上，e为（1，0，0），在第二个特征值上，e 为（0，1，0）,结合U对q的求导【在3）部分中】，得到公式：
q为pj的x,y,z中的其中一个，因此uk对p的求导为：
进一步的替换为：
设置
当m=n的时候C(m,n)为0，因为C是反对称矩阵，对角线元素为0，【看3）中说明】
结合公式(19),
6）得到特征值对点的二阶求导
把公式(19)和公式(23）结合起来，因此二阶导数就出来：
最后作者列了一下这个公式：
其实这个公式不仅用于公式（24），还用于公式（19）的推导。
7）得到点对旋转和平移的求导
这样根据链式求导原理，就可以得到特征值对旋转和平移的求导了。也可以得到二阶和一阶的特征值对旋转和平移的求导了。
8）泰勒二阶展开和求解方程构建
p 是位姿。
J 和H 简化表达为：
对泰勒展开进行求导，用一阶和二阶系数计算结果
至此整个推导完毕。
1310
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
