lego-loam代码分析（1）-地面提取和点云类聚
lego-loam代码分析（1）-地面提取和点云类聚
- 概述
- imageProjecion.cpp
- 获取点云角度范围 （findStartEndAngle()）
- 无序的点云变为有序（projectPointCloud()）
- 地面分割groundRemoval()
- 点云聚类（cloudSegmentation）
- 点云发布
概述
目前最新开源的3d slam算法lego-loam，为loam的改进版。同时另有高人进行了工程优化。
其原作者github
其工程优化的github
个人对其工程优化后的代码和原理进行了基本分析；
包含：
- imageProjecion
- FeatureAssociation
- mapOptmization
- transformFusion
这四个文件
imageProjecion.cpp
imageProjecion.cpp为独立的线程，接收3D 点云数据，主要针对3D激光器采集的激光点云数据，因为代码中间所采用策略原理基本依赖于3d激光雷达扫描测距方式而来。
本线程主要工作是对3d 点云进行预处理，主要将其地面分割、聚类、非聚类、无序点云变有效点云等功能；
点云预处理基本流程：
- 接收新的一帧cloud数据；
- 初始化临时变量数据；
- 将ros cloud 转换成pcl point格式；
- 采用pcl库剔除无效值；
- 统计雷达数据扫描起始角度和终止角度，包括角度差范围；
- 将无序的点云数据，根据3d雷达（如16线 velodyne）的基本扫描原理，转换为有序点云，包括水平index和垂直index。可认为将3d点云转换为2维平面图像存储。
- 地面和非地面分割；
- 点云聚类
- 发布点云
获取点云角度范围 （findStartEndAngle()）
先提供一张图，为vlp雷达驱动中计算点云坐标的方法，同时也是依据此还原序列的点云和距离信息。
从上图可看出，和一般单线激光雷达还有ros中scan的格式有一定差距，主要是水平扫描角度α\alphaα 的表示意义差距较大。因此后续的许多坐标转换都和此有关系，其中α\alphaα为vlp16驱动中提供的扫描角度，因此后续畸变矫正等操作涉及扫描时刻和扫描方向也与此有关；
// 查找整个点云数据起始角度和终止角度
void ImageProjection::findStartEndAngle() {// start and end orientation of this cloudauto point = _laser_cloud_in->points.front();_seg_msg.startOrientation = -std::atan2(point.y, point.x); // 起始角度point = _laser_cloud_in->points.back();_seg_msg.endOrientation = -std::atan2(point.y, point.x) + 2 * M_PI; // 终止角度 + 360 if (_seg_msg.endOrientation - _seg_msg.startOrientation > 3 * M_PI) { // 起始角度和终止角度 放在0~360之间_seg_msg.endOrientation -= 2 * M_PI;} else if (_seg_msg.endOrientation - _seg_msg.startOrientation < M_PI) {_seg_msg.endOrientation += 2 * M_PI;}_seg_msg.orientationDiff = // 终止与起始角度差_seg_msg.endOrientation - _seg_msg.startOrientation;
}
由于一个激光器是360度扫描，且VLP16其旋转速度和分辨率可调。存在一周所获取的点云个数可能会有一定跳动，且终点有可能大于此次起点的角度，也有可能未超过起点角度，故起始和终点角度在360左右，即可能超出360度，而不是一个准确常数。因此终点时刻的角度需要考虑是否进行2*PI翻转。
注：_seg_msg.startOrientation = -std::atan2(point.y, point.x); 此处角度求解形式与常见不同，主要根据传感器驱动有关。loam源码仅适合于VLP系列，若更换其他雷达，需要改正坐标系相关代码才可使用。其中取负号，猜测应该方便后续的计算，即逆时针旋转与笛卡尔坐标系角度方向一致，而VLP16是顺时针旋转
无序的点云变为有序（projectPointCloud()）
由于点云数据内存储每个点的信息为x，y，z为笛卡尔坐标，但是无法获知每个点云之间的相互关系。由于16线激光雷达实际上是16个激光探头同时旋转360度获取的距离信息。因此原数据为16组和单线激光雷达组成。根据所使用雷达已知参数（如16线雷达，包含16组，起始角度为-15度，终止的角度为15度，；每组中包含1800激光点）；
因此变为有序点云方法如下。
- 获取此点到达激光头的距离
loat range = sqrt(thisPoint.x * thisPoint.x + // 反推点的距离thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);
- 获取此点与x，y轴平面的夹角
float verticalAngle = std::asin(thisPoint.z / range); // 获取z轴的角度
- 获取垂直方向上的索引
int rowIdn = (verticalAngle - _ang_bottom) / _ang_resolution_Y; // 获取 扫描线中的索引号，_ang_bottom为起始角度，_ang_resolution_Y为垂直角度分辨率
4.获取此点与x，y轴平面内与x轴的夹角，并获取水平方向上的索引
float horizonAngle = std::atan2(thisPoint.x, thisPoint.y); // x/y ，范围为-PI～ PI， pi/2 表明为x轴方向int columnIdn = -round((horizonAngle - M_PI_2) / _ang_resolution_X) + _horizontal_scans * 0.5; // ??? 不知道为什么这么绕。 表明x轴方向为中间索引号
如此可获取16个在不同垂直（即俯仰角度）下的16组1维激光数据，并按顺序存储；
注意：理论上3d激光雷达与深度摄像机原始数据本身为有序的2维矩阵，但是ros驱动一般发布的为点云格式数据，因此此操作可认为是将点云格式还原回原始数据。如果针对使用的传感器，可修改相应的ros驱动相应直接发布有序序列。如此可利用激光扫描原理和有序性进行后续的分类和特征提取
地面分割groundRemoval()
for (size_t j = 0; j < _horizontal_scans; ++j) {for (size_t i = 0; i < _ground_scan_index; ++i) { // 仅遍历_ground_scan_indexsize_t lowerInd = j + (i)*_horizontal_scans;size_t upperInd = j + (i + 1) * _horizontal_scans;if (_full_cloud->points[lowerInd].intensity == -1 || // 垂直方向上相邻的两个点有一个存在无效值，？？？？？？？？没看到哪里赋值为无效值,不起任何作用_full_cloud->points[upperInd].intensity == -1) {// no info to check, invalid points_ground_mat(i, j) = -1; // 表明此点无法判断continue;}float dX =_full_cloud->points[upperInd].x - _full_cloud->points[lowerInd].x;float dY =_full_cloud->points[upperInd].y - _full_cloud->points[lowerInd].y;float dZ =_full_cloud->points[upperInd].z - _full_cloud->points[lowerInd].z;float vertical_angle = std::atan2(dZ , sqrt(dX * dX + dY * dY + dZ * dZ)); // 存在bug，我觉的应该是sqrt(dX * dX + dY * dY）// TODO: review this change, 判断前后两点的角度变化在10度内if ( (vertical_angle - _sensor_mount_angle) <= 10 * DEG_TO_RAD) {_ground_mat(i, j) = 1;_ground_mat(i + 1, j) = 1;}}}
- 由于目前存储的是按序存储，由于激光雷达主要为水平安装一定高度，因此１６组雷达中，第７组则一般为水平雷达，因此第７组雷达以下的点云数据有可能为地面点云；注：源码思想是假设地面为平的，同时激光雷达安装也应平行与地面，因此在使用时需要严格按照假设使用，否则无法获得预期效果。同时也是此算法的一个缺点
- 按序存储的16组雷达，实际为可认为是16个上下固定角度间隔布置的单线激光雷达；
- 判断是否为地面的方法为，找到第1组雷达（即朝下的第一组激光点）中每个点，找到对应相邻组中同一水平索引的点。如果此两点的俯仰角变化在一范围内，则认为为同一平面，则为地面上点；
注：水平索引方向相同，从第一行开始判断相邻的点。
4. 提取地面数据，提取非地面和非无效数据；
点云聚类（cloudSegmentation）
点云聚类的目的主要是将相邻较近的点认为为同一物体表面，主要用于后续的特征提取；认为为同一物体的原理，则遍历每个未被分类标记的点进行检测。以当前点开始将其上下，左右四个点分别列入待判断的buffer中，判断一点与其相邻点满足一条件。直到所有点都被分类；
这里的点云类聚仅考虑非无效值和非地面上的其他点云
float d1 = std::max(_range_mat(fromInd.x(), fromInd.y()), // 获取当前点和相邻点，距离较大值_range_mat(thisIndX, thisIndY));float d2 = std::min(_range_mat(fromInd.x(), fromInd.y()), // 获取较小值_range_mat(thisIndX, thisIndY));float alpha = (iter.x() == 0) ? _ang_resolution_X : _ang_resolution_Y; // 根据相邻方向获取水平或垂直方向的角度分辨率float tang = (d2 * sin(alpha) / (d1 - d2 * cos(alpha))); // 实际为短线向长线做垂直线， 长线端点离垂线位置，越近，表明越平坦if (tang > segmentThetaThreshold) { // 越大表明越平坦，表明为同一分类，放入queue，继续扩展分类queue.push_back( {thisIndX, thisIndY } );_label_mat(thisIndX, thisIndY) = _label_count; // 将其标记为同一分类lineCountFlag[thisIndX] = true; // 垂直方向的此行，已分类过all_pushed.push_back( {thisIndX, thisIndY } ); // 是此分类的，均放入放入all pushed}
同一类原理如图所示;
水平方向和垂直方向alpha夹角不同，其中tang的角度越大，表明相邻的两个point越接近在一个平面上，故可认为是同一类；
在分类时还需考虑此物体大小限制，过小则不进行具体分类。其原理：
1.同一类别点个数需超过30个；
2.个数超过5个并且在垂直方向上跨过3个区域（因为垂直方向角度分辨率较大）；
点云发布
- 发布已分类的点云数据，包含地面和非地面数据；其中地面点云在水平方向上被降采样；
- 发布未被分类的点云（即小物体，孤立的点云簇），未被分类的在水平方向上被降采样;
注意：目前lego-loam假设匀速运动，未考虑点云的畸变处理
lego-loam代码分析（1）-地面提取和点云类聚相关推荐
- 代码实战 | 用LeGO-LOAM实现地面提取
编辑丨计算机视觉life 作者介绍:Zach,移动机器人从业者,热爱移动机器人行业,立志于科技助力美好生活. LeGO-LOAM框架设计思路的第一步就是提取并分离地面.本篇文章就来详细说明LeGO-L ...
- 3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析 前言 利用地面点优化 利用角点优化 代码部分 gazebo测试 前言 LeGO-LOAM的全称是 Lightweight an ...
- LeGO LOAM学习
LOAM LOAM是一套非常有价值的LIDAR ODOMOTRY算法(它是一个历程计算法,没有回环检测和全局优化的部分). LEGO LOAM LeGO LOAM 它含有四个主要线程 image pr ...
- lego-loam代码分析（3）-激光里程计
lego-loam代码分析(3)-激光里程计 匹配初始化 TransformToStart TransformToEnd 两次LM匹配 平面匹配 匹配点查找 目标点到匹配平面的距离 LM求解 角点匹配 ...
- LOAM 代码部分的公式推导（前端里程计部分）
作者丨小飞@知乎 来源丨https://zhuanlan.zhihu.com/p/404326817 编辑丨3D视觉工坊 本篇主要介绍LOAM代码中有关lidar odometry部分对应的公式推导. ...
- starGAN原理代码分析
下载: git clone https://github.com/yunjey/StarGAN.git 1 cd StarGAN/ 1 下载celebA训练数据: bash download.sh 1 ...
- angular代码分析之异常日志设计
angular代码分析之异常日志设计 错误异常是面向对象开发中的记录提示程序执行问题的一种重要机制,在程序执行发生问题的条件下,异常会在中断程序执行,同时会沿着代码的执行路径一步一步的向上抛出异常,最 ...
- 模块加载过程代码分析1
一.概述 模块是作为ELF对象文件存放在文件系统中的,并通过执行insmod程序链接到内核中.对于每个模块,系统都要分配一个包含以下数据结构的内存区. 一个module对象,表示模块名的一个以null ...
- matlab的NLP功能,pyhanlp 共性分析与短语提取内容详解
pyhanlp 共性分析与短语提取内容详解 简介 HanLP中的词语提取是基于互信息与信息熵.想要计算互信息与信息熵有限要做的是 文本分词进行共性分析.在作者的原文中,有几个问题,为了便于说明,这里首 ...
最新文章
- 【知识积累】SBT+Scala+MySQL的Demo
- nboot通过DNW下载并运行eboot.nb0
- 8.3 matlab图形用户界面设计方法
- ML：MLOps系列讲解之《MLOps原则之测试》解读
- esxi服务器接移动硬盘,esxi添加usb移动硬盘存储数据
- spring框架搭建第二天
- JavaScript快速学习
- java 反射解析xml_java反射获取xml元素
- bootstrap table 分页只显示分页不显示总页数等数据
- 关于钥匙串中所有证书签名无效的问题解决纪录
- centos搭建git服务
- mysql explain ref列_mysql explain中的列
- JS：关于JS字面量及其容易忽略的12个小问题
- javascript 权威指南一
- 2021年计算机网络工程师真题,2021年计算机四级网络工程师题库完整版完整答案.doc...
- LTE：资源调度（5）
- Springboot配置文件
- 扇贝python多少钱_扇贝多少钱一斤？扇贝多少钱一斤2017？
- 怎样组织一次攻防演练比赛- 前期准备阶段
- 浅谈如何保障服务器安全