LOAM 代码部分的公式推导（前端里程计部分）

会员服务 ·

LOAM 代码部分的公式推导（前端里程计部分）

2021 年 9 月 5 日 计算机视觉life

点击上方“计算机视觉life”，选择“星标”

快速获得最新干货

作者丨小飞@知乎

来源丨https://zhuanlan.zhihu.com/p/404326817

转载自3D视觉工坊

本篇主要介绍LOAM代码中有关lidar odometry部分对应的公式推导。包含特征点曲率计算公式，点到线和面距离求取公式，里程计优化求解公式以及一些坐标变换公式。LOAM代码版本为 https://github.com/laboshinl/loam_velodyne

（1）特征点曲率计算

相关代码：

按照算法流程，需要根据提取的特征点，来建立点到线和点到面的约束关系即点到线的距离和点到面的距离最小。代码中求取两个距离的原理公式如下：

按照上述所讲内容，结合代码（x0,y0,z0）代表上图中i点坐标，（x1,y1,z1）和（x2,y2,z2）分别代表 j 和 l 点坐标。

同理，点到面的距离也可以按照法向量的方式来求取，相关代码如下：

结合代码带入上述公式，i 点坐标为代码中的（pointSel.x, pointSel.y, pointSel.z）；j 点坐标为代码中的（tripod1.x, tripod1.y, tripod1.z）；l点坐标为代码中的（tripod2.x, tripod2.y, tripod2.z）；m 点坐标为代码中的（tripod3.x, tripod3.y, tripod3.z）。

那么

（3）里程计优化求解公式

在进行里程计求解公式推导前，需要了解其中几个重用的坐标系转换。

备注：感谢微信公众号「 3D视觉工坊 」整理。

其中transformToStart 对应论文中的公式

注意LOAM代码中point.intensity在预处理时被存储为(scanID+relTime)

transformToEnd 函数中就是先做了transformToStart ,再将点云全部都转换到一帧的结束时刻。

对于里程计的优化求解部分，我们先看一下整体思路：

LOAM中利用建立点到线和点到面的距离最小值来作为约束方程，最后利用L-M非线性优化方法来求解这个非线性问题。对于L-M方法，用到的公式如下：

首先直接建立约束方程的是已经坐标转换后的点云坐标即X

那么根据链式法则，我们就可以先通过约束方程对坐标进行求偏导，然后再乘以坐标对变换中的rotX求偏导，即可得到最终的偏导求取结果。

首先根据约束方程对坐标量求偏导，我们上文已经建立了约束方程。

这里的la lb lc 就是点到线距离的函数对于X Y Z的偏导数，这里面可以通过导数的几何意义来理解，导数在几何意义上表示梯度。而梯度方向就是我们所求的距离函数的单位向量了，也就是下图的De 方向。

代入后就得到代码中的 la lb lc ，但这里推导的公式正负号也和代码中有所不同；同理，点到面约束方程求偏导也是按照此种方式求得。

最后一个公式为里程计累加，函数代码如下图：

以上具体内容可以关注本人“彻底搞懂基于LOAM框架的3D激光SLAM:源码剖析到算法优化”课程视频，具体课程介绍可以扫以下二维码：

独家重磅课程！

1、重磅来袭！基于LiDAR的多传感器融合SLAM 系列教程：LOAM、LeGO-LOAM、LIO-SAM

2、系统全面的相机标定课程：单目/鱼眼/双目/阵列相机标定：原理与实战

3、视觉SLAM必备基础课程：视觉SLAM必学基础：ORB-SLAM2源码详解

4、深度学习三维重建课程：基于深度学习的三维重建学习路线

5、激光定位+建图课程：激光SLAM怎么学？手把手教你Cartographer从入门到精通！

6、视觉+IMU定位课程视觉惯性里程计58讲教程全部上线！IMU预积分/残差雅克比推导、边缘化约束、滑窗BA！

7、手把手图像三维重建课程：系统学三维重建42讲全部上线！掌握稠密匹配、点云融合、网格重建、纹理贴图！

全国最棒的SLAM、三维视觉学习社区↓

技术交流微信群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群，请添加微信号 chichui502 或扫描下方加群，备注：”名字/昵称+学校/公司+研究方向“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~