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

作者丨小飞@知乎

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

编辑丨3D视觉工坊

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

(1)特征点曲率计算

相关代码：

通过求取这部分代码diffX * diffX diffY * diffY diffZ * diffZ 近似表示曲率，角点和平面点通过曲率小划分。其中 _config.curvatureRegion 设置为5，表示将该点左右各5个点作为邻域范围来获取曲率。因此pointWeight = -2 * _config.curvatureRegion = -10.将其带入循环公式：

diffX = -10*_laserCloud[i].x _laserCloud[i 1].x _laserCloud[i-1].x _laserCloud[i 2].x _laserCloud[i-2].x … _laserCloud[i 5].x _laserCloud[i-5].x

重组成如下公式：

diffX =（_laserCloud[i 1].x - _laserCloud[i].x） (_laserCloud[i-1].x - _laserCloud[i].x) … (_laserCloud[i 5].x - _laserCloud[i].x) (_laserCloud[i-5].x - _laserCloud[i].x)

最后得到左右各5个点邻域的该点diffX.

(2)点到线，面距求解公式

相关代码：

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

根据上述内容，结合代码（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

在「3D视觉工坊」微信官方账号后台回复:3D视觉，即可下载 3D干货的视觉相关数据涉及相机校准、三维重建、三维视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」微信官方账号后台回复:3D视觉github资源汇总，可下载包括结构光、校准源代码、缺陷检测源代码、深度估计和深度补充源代码、点云处理相关源代码、三维匹配源代码、单目、双目3D检测，基于点云的3D检测、6D估计源码汇总的姿态等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，独家下载即可相机标定学习课件和视频网站；背景回复:立体匹配，独家下载即可立体匹配学习课件和视频网站。

重磅！3DCVer-学术论文写作投稿交流群已成立

扫码添加小助手微信，可申请加入3D视觉研讨会-学术论文写作和提交微信交流群旨在交流顶会、顶刊、SCI、EI等待写作和提交。

同时也可以申请加入我们的细分交流群。目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶，多传感器集成，CV入门，三维测量，VR/AR、3D人脸识别、医学影像、缺陷检测、行人重识别、目标跟踪、视觉产品着陆、视觉竞赛、车牌识别、硬件选择、学术流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~