本文介绍了一种基于BA算法优化LOAM的方案，通过构建扫描间的特征匹配，并结合平面和直线约束，实现对激光雷达里程计位姿的进一步优化。文章详细探讨了代价函数、雅可比矩阵和海森矩阵的计算方法。
根据点在直线或平面上的约束，构建扫面间的特征匹配，基于此使用BA算法来进一步优化LOAM等视觉里程计得到的位姿。采用了非常巧妙的方式计算直线法向量相较于位姿变化的导数。
代价函数，雅可比矩阵和海森矩阵
在已知点和对应直线对应关系的前提下，整体代价函数表述为：
( T ∗ , n ∗ , q ∗ ) = arg min T , n , q 1 N ∑ i = 1 N ( n T ( p i − q ) ) 2 = arg min T ( min n , q 1 N ∑ i = 1 N ( n T ( p i − q ) ) 2 ) \begin{aligned}\left(\mathbf{T}^{*}, \mathbf{n}^{*}, \mathbf{q}^{*}\right) &=\arg \min _{\mathbf{T}, \mathbf{n}, \mathbf{q}} \frac{1}{N} \sum_{i=1}^{N}\left(\mathbf{n}^{T}\left(\mathbf{p}_{i}-\mathbf{q}\right)\right)^{2} \\ &=\arg \min _{\mathbf{T}}\left(\min _{\mathbf{n}, \mathbf{q}} \frac{1}{N} \sum_{i=1}^{N}\left(\mathbf{n}^{T}\left(\mathbf{p}_{i}-\mathbf{q}\right)\right)^{2}\right) \end{aligned} (T∗,n∗,q∗)=argT,n,q
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
