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天内无条件退款

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

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