LOAM算法（论文+代码）详解（一）—— 引言+特征提取
目录
引言
符号定义
一、算法流程
二、雷达里程计
1. 特征提取
1.1 平滑度计算
1.2 特征提取
引言
LOAM: Lidar Odometry and Mapping in Real-time（RSS2014），是常年位于KITTI上前列的激光雷达SLAM算法，作为该领域发表时间较早的论文，启发了后续很多类似的工作。理解LOAM有助于初学者理解激光雷达SLAM的基本原理和整体流程。本系列文章将为各位同学系统梳理LOAM算法论文和代码中涉及的重点和难点。
图1 LOAM在KITTI上的排名
传送门：
论文：www.ri.cmu.edu/pub_files/2014/7/...
原味代码：github.com/laboshinl/loam_velodyne
改进代码：github.com/HKUST-Aerial-Robotics（使用Ceres_Solver库做非线性优化，简化了代码结构）。
论文亮点：
- 雷达运动畸变的补偿；
- 特征提取减小了点云配准的数据量；
- ICP使用了非线性迭代优化（LM方法），而不是传统的SVD分解。
符号定义
在正式讲解算法之前，首先要搞清楚原文中的两个名词——scan和sweep。由于时间较为久远，原作者使用的激光雷达型号为Hokuyo（北阳） UTM-30LX，是一个2D激光线扫设备，一次只能获取一条的视角范围为间距
的激光线。作者将其连接到一个旋转电机上，使其可以在竖直方向做
的旋转，一秒钟可以生成40条扫描线，因此在竖直方向上的分辨率为
。原文中，scan代指水平方向上的一条扫描线，sweep代指雷达完成一次半球旋转后得到的扫描面。
将第k次扫描sweep记作k，。第k次扫描内获取的点云数据记为
。雷达坐标系满足右手坐标系,坐标系原点位于雷达的几何中心，x轴正方向向左，y轴向上，z轴向前，表示为
。第k次扫描的雷达观测坐标系表示为
，
中的一个点
，在
中表示为
。世界坐标系表示为
，与初始位置的观测坐标系
重合，
中的一个点
，在
中可以表示为
。
那么，要求解的问题可以表述为：
给定一个雷达点云序列，
，计算雷达在k次扫描内的运动，并利用
构建已遍历到的场景的地图。
一、算法流程
图2 LOAM算法流程图
算法整体可以分为三个部分：
- Point Cloud Registration: 可以理解为点云的一个预处理步骤，区分采集点云的通道（竖直方向方位角）和时刻（水平方向方位角），并进行特征点提取；
- Lidar Odometry: 雷达里程计，对同一帧（sweep）内不同时刻采集的点云(scan)做运动变形（motion distortion）的补偿，得到无变形的单帧点云数据；
- Lidar Mapping: 雷达建图，将每一帧的点云(sweep)配准到全局坐标系下，并进行体素抽稀，得到三维点云地图。
对于每帧输入的点云，（1）首先，经过点云预处理和特征提取步骤（Point Cloud Registration），得到
；（2）其次，通过雷达里程计（Lidar Odometry）对第k帧点云
做变形补偿，得到无运动变形的点云
，该步骤的更新频率为10Hz;（3）再次，通过雷达建图（Lidar Mapping），将每一帧的无变形点云
配准到全局坐标系
下，并进行体素抽稀，得到三维点云地图，该步骤更新频率为1Hz；（4）最后，集成前两个步骤发布的姿态变换，生成相对于地图的当前激光雷达姿态变换，输出频率为10Hz。
二、雷达里程计
雷达里程计包括三个主要模块：特征提取、特征匹配和运动估计。下面对这几个模块分别进行介绍。
1. 特征提取
1.1 平滑度计算
在进行特征提取之前，需要对当前帧点云中的每个点i计算其平滑度，平滑度计算公式：
其中，当前点i的坐标为，
为与当前点i相邻的前后n个点组成的集合，
为邻域集合
中的一个点j的坐标。
for (int i = 5; i < cloudSize - 5; i++){ float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;//前后5个点与当前点的差的平均值*10cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;cloudSortInd[i] = i;cloudNeighborPicked[i] = 0;cloudLabel[i] = 0;}A-LOAM平滑度计算对应代码段
1.2 特征提取
LOAM算法将点云特征分为两类：平面点和边缘点。
- 平面点(planar points)：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。
- 边缘点(edge points)：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。
对当前帧内的点进行排序，找到c 最小的点作为平面点，c 最大的点作为边缘点。为了使特征值点分布均匀，将一帧扫描（sweep）内的一条扫描线（scan）分成四段，每一段提取最多两个边缘点，四个平面点。此外，如下图所示这两种不可靠点也不会被选为特征点。
图3 不可靠点示意图。（a）A点所在平面与雷达发出的激光束呈一定夹角，B点所在平面与激光束几乎平行，B点为不可靠点；（b）B点与A点的间隔较大，且线段AB与激光线束平行，B点对A点造成了遮挡，A点为不可靠点。
特征点选取的准则总结如下：
- 不能超过设定的size，每个集合平面点4个，边缘点2个；
- 已选取的点周围不能有点，使得点可以分布的更加均匀；
- 选取的平面点不能与激光扫描束平行。
for (int i = 0; i < N_SCANS; i++)//通道数{if( scanEndInd[i] - scanStartInd[i] < 6)continue;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);for (int j = 0; j < 6; j++)//每条扫描线分为六段{int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; //起点IDint ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;//终点IDTicToc t_tmp;std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);//用cloudCurvature排序t_q_sort += t_tmp.toc();int largestPickedNum = 0;for (int k = ep; k >= sp; k--){int ind = cloudSortInd[k]; if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1)//C值较大为边缘点{largestPickedNum++;if (largestPickedNum <= 2)//C值最大的两个点构成边缘点小集合Fe，参考LeGO-LOAM{ cloudLabel[ind] = 2;cornerPointsSharp.push_back(laserCloud->points[ind]);cornerPointsLessSharp.push_back(laserCloud->points[ind]);}else if (largestPickedNum <= 20)//C值较大的20个点构成边缘点大集合Fme，参考LeGO-LOAM{ cloudLabel[ind] = 1; cornerPointsLessSharp.push_back(laserCloud->points[ind]);}else{break;}cloudNeighborPicked[ind] = 1; //取过的点不再被取for (int l = 1; l <= 5; l++){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;//间隔很大的邻近点仍然可以被选}cloudNeighborPicked[ind + l] = 1;//当前点的后5个点不再被取}for (int l = -1; l >= -5; l--){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;//间隔很大的邻近点仍然可以被选}cloudNeighborPicked[ind + l] = 1;//当前点的前5个点不再被取}}}int smallestPickedNum = 0;for (int k = sp; k <= ep; k++){int ind = cloudSortInd[k];if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1)//C值较小为平面点{cloudLabel[ind] = -1; surfPointsFlat.push_back(laserCloud->points[ind]);smallestPickedNum++;if (smallestPickedNum >= 4) // C值最小的两个点构成平面点小集合Fp，参考LeGO-LOAM{ break;}cloudNeighborPicked[ind] = 1; //取过的点不再被取for (int l = 1; l <= 5; l++){ float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;//间隔很大的邻近点仍然可以被选}cloudNeighborPicked[ind + l] = 1; // 当前点的后5个点不再被取}for (int l = -1; l >= -5; l--){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;//间隔很大的邻近点仍然可以被选}cloudNeighborPicked[ind + l] = 1;//当前点的前5个点不再被取}}}for (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0) // C值小于条件阈值的其余点构成平面点大集合Fmp，参考LeGO-LOAM{surfPointsLessFlatScan->push_back(laserCloud->points[k]);}}}pcl::PointCloud<PointType> surfPointsLessFlatScanDS;//平面点大集合Fmp做体素抽稀pcl::VoxelGrid<PointType> downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.filter(surfPointsLessFlatScanDS);surfPointsLessFlat += surfPointsLessFlatScanDS;//每段的平面点大集合Fmp存到当前帧的总集合中}A-LOAM特征点选取对应代码段
LOAM算法（论文+代码）详解（一）—— 引言+特征提取相关推荐
- Alphapose论文代码详解
注:B站有相应视频,点击此链接即可跳转观看https://www.bilibili.com/video/BV1hb4y117mu/ 第1节 人体姿态估计的基本概念 第2节:Alphapose 2.1A ...
- SLAM-Visual Navigation学习之SIFT算法与代码详解
** SIFT算法 ** 文章目录 SIFT算法 一.特征点,关键点,角点? 二.前置知识 1.尺度 2.卷积 3.高斯函数 4.高斯卷积(模糊) 三.SIFT算法的引入 Harris算法缺陷: 1. ...
- 特征点检测 FAST算法及代码详解
本文着重介绍了用于图像特征点检测的算法,FAST算法,以及使用matlab的实现. FAST算法是一种拐点检测算法,其主要应用于提取图像中的特征点,在动态成像的一系列图像中追踪定位对象.众所周知,我们 ...
- 《机器学习实战》第二章学习笔记：K-近邻算法（代码详解）
<机器学习实战>数据资料以及总代码可以去GitHub中下载: GitHub代码地址:https://github.com/yangshangqi/Machine-Learning-in-A ...
- 鲸鱼算法matlab代码详解（一）
主函数 clear all clc SearchAgents_no=30; %此处为搜索代理的数量,也就是种群的数量 Function_name='F1'; %此处为调用目标函数的信息编号 Max_ ...
- SSD算法-论文阅读详解
目录 SSD: Single Shot MultiBox Detector(单点多盒检测器) 链接:SSD论文原文 一,贡献/创新点 1,SSD是一个single-shot多类检测器,比之前的单点检测 ...
- MeanTeacher文章解读+算法流程+核心代码详解
MeanTeacher 本博客仅做算法流程疏导,具体细节请参见原文 原文 原文链接点这里 Github 代码 Github代码点这里 解读 论文解读点这里 算法流程 代码详解 train_transf ...
- c语言实现sha1算法注解,【密码学】SHA1算法实现及详解
1 SHA1算法简介 安全哈希算法(Secure Hash Algorithm)主要适用于数字签名标准(Digital Signature Standard DSS)里面定义的数字签名算法(Digit ...
- 标准oc算法的推导与99行代码详解
文章目录 标准oc算法的推导与代码详解 问题描述 OC算法的数学描述 结果展示 OC算法的matlab代码及注释 参考文献 标准oc算法的推导与代码详解 对于变密度的参数化方法,设计变量x为材料相对密 ...
- 【OpenCV/C++】KNN算法识别数字的实现原理与代码详解
KNN算法识别数字 一.KNN原理 1.1 KNN原理介绍 1.2 KNN的关键参数 二.KNN算法识别手写数字 2.1 训练过程代码详解 2.2 预测分类的实现过程 三.KNN算法识别印刷数字 2. ...
最新文章
- JVM---运行时数据区概述
- 千呼万唤，ACS始出来
- 《黑马程序员》认识OC的第一个程序（Objective-c）
- 物理隔离已是过去 工控网络如何更好地保护SCADA
- rtsp协议_如何在RTSP协议视频智能平台EasyNVR未登录的情况下调用通道直播的接口？...
- access数据库拆分的用途_聊聊数据库设计一些经验 条条都是干货
- 如何解决error: failed to push some refs to ‘xxx(远程库)‘
- php上传文件后无法移动到指定目录的解决
- AI工程师面试凭高频问题提前准备，命中率会是多少？
- ios 不同sdk4.3 6.0版本号，关于方法的兼容性的通用方法
- 远程工具：MobaXterm使用图文教程
- android光传感实现摩斯密码,根据莫尔斯代码 - Android的闪烁闪光。 如何避免ANR次数由于睡觉？ （火炬APP）...
- matlab运行为什么要选中代码,性能 – 为什么(在MATLAB中)这个代码更快？
- 多臂老虎机导论(二)Stochastic Bandits
- break 和 continue
- linux下木马程序病原体的制作和运行
- 微信手写板 android,微信小程序：手写板功能实现（canvas）
- 一个纸杯子的测试用例
- 自控力读书笔记：第三章 累到无力抵抗：为什么自控力和肌肉一样有极限？
- php的png乱码,如何解决php png乱码问题