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天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用,麻烦给个赞和在看~
LOAM 代码部分的公式推导(前端里程计部分)相关推荐
- 3D激光雷达SLAM算法学习03——前端里程计方案
1.本篇大纲 2.前端里程计综述 解决的问题:由原始点云信息求解得到两个关键帧之间的相对位姿约束 输入:原始点云信息 输出:两个时刻之间的位姿关系 P = [R T] (4x4) 主流流程: 点云预处 ...
- [KITTI] EVO评价前端里程计精度
言以, 事以 1. 介绍 2. 安装 3. 使用 3.1 注意 3.2 样例数据说明 3.3 使用 3.3.1 每段距离误差(相对位姿误差) 结果 3.3.2 总累计误差(绝对位姿误差) 结果 1. ...
- 【多传感器融合定位SLAM专栏】前端里程计、IMU预积分、滤波、图优化推导与应用(3)
本专栏基于深蓝学院<多传感器融合定位>课程基础上进行拓展,对多传感器融合SLAM的学习过程进行记录 文章目录 第三章 惯性导航原理及误差分析 惯性器件 1. 机械陀螺 2. 激光陀螺 ME ...
- 3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析
3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析 前言 利用地面点优化 利用角点优化 代码部分 gazebo测试 前言 LeGO-LOAM的全称是 Lightweight an ...
- LOAM:实时的雷达里程计和建图
之前对视觉SLAM主要的开源框架,ORB-SLAM2.SVO等进行了介绍,然后疫情期间对VINS-Mono进行了详细的源码解析,接下来考虑到工作原因需要用到激光雷达.GNSS.IMU等多传感器融合,所 ...
- 深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...
- LiLi-OM: 走向高性能固态激光雷达惯性里程计和建图系统
点击上方"计算机视觉工坊",选择"星标" 干货第一时间送达 编辑丨当SLAM遇见小王同学 声明: 本文只是个人学习记录,侵权可删.论文版权与著作权等全归原作者所 ...
- svo: semi-direct visual odometry 半直接视觉里程计 fast角点匹配 光流匹配 单应变换求位姿 直接法求解位姿 高斯均匀分布混合深度滤波
svo: semi-direct visual odometry 半直接视觉里程计 本博文github地址 svo代码注释 SVO代码分析 较细致 svo: semi-direct visual od ...
- 【SLAM】LIO-SAM解析——获得高频的lidar里程计TransformFusion(6)
系列文章链接: [SLAM]LIO-SAM解析--流程图(1) [SLAM]LIO-SAM解析--数据预处理imageProjection(2) [SLAM]LIO-SAM解析--特征提取featur ...
最新文章
- 【CNN】很详细的讲解什么以及为什么是卷积(Convolution)!
- 10种Git技巧,让你省时省力又省心!
- 【C#】详解C#委托
- 如何判断hadoop包tar.gz64位还是32位
- java 调用一个抛出异常的函数,Java VS C++(14) 调用可能抛出异常的函数
- hdfoo站点开发笔记
- Graphviz从入门到不精通
- .sql文件如何执行_干货|一条SQL查询语句是如何执行的
- Kali [masscan]端口扫描
- 大作手操作体系:突破后回调不下箱体可加仓,但是也有失误的时候!
- 分布式计算 网格计算 并行计算 云计算
- yolov5训练时的dataset not found
- java输出格林威治时间,Java之格林威治时间格式转换成北京时间格式
- 714.买卖股票含手续费
- 基于评论的跨境电商产品满意度分析_kaic
- 新生儿取名:撩人于无形的女宝宝名字,任你挑选
- 简单实用,聊聊我常用的 4 款 Pandas 自动数据分析神器
- node内存溢出问题 Javascript Heap out of memory
- C10K 问题引发的技术变革
- Date日期格式转换