ROS-3DSLAM（4）：lidar_odometry包浅析

2021@SDUSC

2021年10月11日星期一——2021年10月1４日星期四

一、学习背景：

结束了论文的学习之后，我们下一步的学习目标就是关键的代码了，但是在此之前，我们还仍然需要先把整个项目的信息流分析明白，这样才能有助于我们更好地进行后续的阅读学习代码工作。

结束了信息流的分析之后，我们的第二个工作目标就是对于ｌｉｄａｒ＿ｏｄｏｍｅｔｒｙ包的代码分析。相较于第一个视觉处理的系统而言，这个雷达处理的系统更为简单一些，分析起来也相对较为容易。我们首先分析的目标是较小的三个文件，剩下的那一个较大的代码文件：图优化，我们下一周再处理。这一周，我负责的部分是特称提取的功能，也就是“featureExtraction.cpp”。

因为第二项工作任务较多，所以考虑到篇幅，所以我把信息流的部分摘取了出来，单独列为了一篇文章，下一次再写出来。

二、特征提取（featureExtraction.cpp）：

１、开头导入文件：

代码开头导入的两个文件当中，一个是本地文件夹下的ｕｔｉｌｉｔｙ，一个是ｌｖｉ－ｓｌａｍ下的ｃｌｏｕｄ＿ｉｎｆｏ。这里特别要注意的是第二个头文件。第一个头文件中包含了本系统中所需要的各种初始定义，位于本地文件夹下，而第二个头文件则是点云的基本信息，除此之外，也包含了“imu”和“odom”的成员变量。

之后，作者又定义了两个数据结构，分别用来存储和计算（）信息。

2、基本的定义：

在这个大类当中，作者首先给出了相应的定义。

因为是首次接触点云这个概念，而且长时间没有接触cpp，有些陌生，所以阅读代码起来稍微有些吃力。

因此我参考了知乎上的注释来入手（见文末）。在这里，注释者基本上介绍了作者缩写代码的思路。

ros::下：创建了订阅者Subscriber和发布者Publisher，其中发布者有三个，分别是pubLaserCloudInf，pubCornerPoints和pubSurfacePoints，他们分别用来完成发布不同的点云信息：激光帧提取特征之后的点云信息，激光帧提取到的角点点云以及平面点点云。
```
ros::Subscriber subLaserCloudInfo;

ros::Publisher pubLaserCloudInfo;
ros::Publisher pubCornerPoints;
ros::Publisher pubSurfacePoints;
```
pcl::下：这里有牵扯到pcl这个概念，所以需要说明一下。
- pcl：是一种数据结构，在ROS最新版本中用来存储点云的信息。
- 而这里作者给出的定义也符合它的功能，设计了三个集合来存储点云的信息：extractedCloud、cornerCloud和surfaceCloud。
- ```
pcl::PointCloud<PointType>::Ptr extractedCloud;
pcl::PointCloud<PointType>::Ptr cornerCloud;
pcl::PointCloud<PointType>::Ptr surfaceCloud;

pcl::VoxelGrid<PointType> downSizeFilter;
```
lvi-sam和std_msgs下的两个定义变量用来存储当前激光帧的全部信息，包括所有的历史数据。
```
lvi_sam::cloud_info cloudInfo;
std_msgs::Header cloudHeader;
```
这个地方我不太理解，lvi_sam下是什么？难道这是本包下的基础类型？他是怎么储存所有的历史数据的？
std::vector与Float变量下的集合用来存储激光帧点云的曲率。
```
std::vector<smoothness_t> cloudSmoothness;
float *cloudCurvature;
```
*cloudNeighborPicked
int变量一个是标记，一个是标签，分别用来表示是否提取和提取的类型：角点还是平面点。
```
 int *cloudLabelint *cloudNeighborPicked
```

2、函数分析一：初始化

首先的两个函数 FeatureExtraction和 initializationValue就是用来初始化的。

它们初始化了订阅者和发布者的信息，并且给上文所定义的各个变量进行了初始化。

FeatureExtraction()
{// 订阅当前激光帧运动畸变校正后的点云信息subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧提取特征之后的点云信息pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);// 发布当前激光帧的角点点云pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);// 发布当前激光帧的面点点云pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);// 调用初始化函数initializationValue();
}

// 初始化各个变量信息
void initializationValue()
{cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);
extractedCloud.reset(new pcl::PointCloud<PointType>());cornerCloud.reset(new pcl::PointCloud<PointType>());surfaceCloud.reset(new pcl::PointCloud<PointType>());
cloudCurvature = new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];cloudLabel = new int[N_SCAN*Horizon_SCAN];
}

3、核心函数群的功能分析：

1 laserCloudInfoHandler

开头的三行将得到的数据存储在了之前声明定义的cloudinfo、cloudHeader和pcl::fromROSMsg之中。

pcl::fromROSMsg这一行还有得到当前激光帧畸变校正之后的有效点云的功能……我没看出来

之后就是调用的核心函数群。下面依次介绍。

  void laserCloudInfoHandler(const lvi_sam::cloud_infoConstPtr& msgIn)

{
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction

calculateSmoothness();

markOccludedPoints();

extractFeatures();

publishFeatureCloud();
}

2 calculateSmoothness

不严谨的翻译：计算平滑度？

用于计算当前激光帧中每个点的曲率。

为什么要这么算？

 void calculateSmoothness(){int cloudSize = extractedCloud->points.size();//遍历所有的有效点云for (int i = 5; i < cloudSize - 5; i++){//用当前激光点前后5个点计算当前点的曲率float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]+ cloudInfo.pointRange[i+5];           cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudNeighborPicked[i] = 0;cloudLabel[i] = 0;// cloudSmoothness for sorting//存储该点的曲率、激光点的一维索引cloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}}

3 markOccludedPoints

标记闭塞的点？

结合作者给出的注释，和其他人的介绍可以得知，这个函数的功能是用来标记两种不同的点。分别是被遮挡的和被标记的。

 void markOccludedPoints(){int cloudSize = extractedCloud->points.size();// mark occluded points and parallel beam pointsfor (int i = 5; i < cloudSize - 6; ++i){// occluded pointsfloat depth1 = cloudInfo.pointRange[i];float depth2 = cloudInfo.pointRange[i+1];int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
if (columnDiff < 10){// 10 pixel diff in range imageif (depth1 - depth2 > 0.3){cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}else if (depth2 - depth1 > 0.3){cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}// parallel beamfloat diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])cloudNeighborPicked[i] = 1;}}

4 extractFeatures（最为核心的函数）

特征提取：名字如同本类的名字，其重要性可见一斑。

首先清除原来的信息（clear()），并创建智能指针ptr。

第一层for循环：N_SCAN 是在utility.h头文件中定义的变量，经过查阅头文件和相关资料可以得知这是存储了雷达中的数据：扫描线。因此第一个for循环的作用是遍历所有的扫描线。
第二个for循环：6是一个数字常量，这里是作者用来分段的数目，将每一个SCAN分为六段来进行分析

段分开提取有限数量的特征，保证特征均匀分布。——为什么要分成六段？
用线性插值对 SCAN 进行等分，取得 sp 和 ep，即 start point 和 end point

startRingIndex为扫描线起始第5个激光点在一维数组中的索引

文末给出了什么线性插值的博客链接。简单地说就是利用点来反过来估测函数。

至于为什么是第五个激光点？没有搞懂。
sort就是由小到大排列点，排序
第三个for循环：由小到大遍历获取到的点云信息，ep和sp都是刚才所获取到的信息。注意下面还有一个由大到小遍历，注意区分。

为什么要这样遍历两次？

一次是平面点和未被处理的点，一次是曲面点。
ind是一个标签，为了避免重复访问点云而设置。
if判别一：

 // 当前激光点还未被处理，且曲率大于阈值，则认为是角点// 此处 edgeThreshold 默认值为 0.1，即距离大于 0.1

if判别二：在每一段中只提取出来20个点

然后将其标记为角点，加入角点点云。

为什么只取出来20个？
之后标记为已处理。
第四个for循环： // 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
第五个for循环：同上

这两个for循环怎么区分？什么意思，为什么一个是正的，一个是负的游标？
然后是和上边类似的标记、处理、加入、标记。
平面点云降采样，加入集合

什么是降采样？

文末给出了相关的介绍博客，但至于为什么还不是很清楚。

void extractFeatures(){cornerCloud->clear();surfaceCloud->clear();
pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCAN; i++){surfaceCloudScan->clear();
for (int j = 0; j < 6; j++){
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)continue;
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;for (int k = ep; k >= sp; k--){int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold){largestPickedNum++;if (largestPickedNum <= 20){cloudLabel[ind] = 1;cornerCloud->push_back(extractedCloud->points[ind]);} else {break;}
cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++){int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--){int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}
for (int k = sp; k <= ep; k++){int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold){
cloudLabel[ind] = -1;cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));if (columnDiff > 10)break;
cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));if (columnDiff > 10)break;
cloudNeighborPicked[ind + l] = 1;}}}
for (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0){surfaceCloudScan->push_back(extractedCloud->points[k]);}}}
surfaceCloudScanDS->clear();downSizeFilter.setInputCloud(surfaceCloudScan);downSizeFilter.filter(*surfaceCloudScanDS);
*surfaceCloud += *surfaceCloudScanDS;}}

5 freeCloudInfoMemory和publishFeatureCloud

这两个函数的功能很简单，就是用来清理点云和发布点云信息。

后者在执行时会调用前者，先清理点云信息，再保存好新提取到的点云信息，然后传送给图优化函数mapOptimization。

void freeCloudInfoMemory(){cloudInfo.startRingIndex.clear();cloudInfo.startRingIndex.shrink_to_fit();cloudInfo.endRingIndex.clear();cloudInfo.endRingIndex.shrink_to_fit();cloudInfo.pointColInd.clear();cloudInfo.pointColInd.shrink_to_fit();cloudInfo.pointRange.clear();cloudInfo.pointRange.shrink_to_fit();}
void publishFeatureCloud(){// free cloud info memoryfreeCloudInfoMemory();// save newly extracted featurescloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, "base_link");cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");// publish to mapOptimizationpubLaserCloudInfo.publish(cloudInfo);}

ZHOU3 ZHOU4

三、问题汇总及解答

说明：

本来想一个一个处理完成，但是结果发现这些问题自己能够解决的了了无几，因为这几个问题中除了我能解决的之外，剩下的并不与代码的理解相关，还设计到另外的包的内容，毕竟作者是从前边的包的基础上研发的这个系统，因此学习前边的相关的内容的包也是有必要的。如果弄懂了前几个包的内容，说不定后边的这些问题可能就会迎刃而解了。所以我决定先把这些问题能搁置的搁置，先去阅读一下前边的包，并与队友们讨论一下再来处理这些问题。

问题1

答：这是一个基础问题，主要是涉及到cpp语法和点云这一基本结构。

::是域运算符，用来强调声明的是在lvi_sam和std_msgs包下的数据类型点云信息和Header。

cloud_info顾名思义是用来存储点云信息的，而Header则是std_msgs包下用来传递数据的点云信息。

四、参考资料：

LVI-SAM代码注释 – xuwuzhou的SLAM之路 – 静心，慎思，明辨，笃学

Documentation - ROS Wiki

LVI_SAM源码详解 - 知乎 (zhihu.com)

绪论：什么是点云？ - 知乎 (zhihu.com)

pcl的初步使用(ROS)_dxmcu的专栏-CSDN博客

(2条消息) PCL之指针Ptr详解libaiicon的博客-CSDN博客pcl指针

Smart Powerful Lidar Solutions | Velodyne Lidar

velodyne VLP-16 ros下获取点云数据（超详细教程） - 古月居 (guyuehome.com)

(2条消息) 我与插值萍水相逢：线性插值(Linear Interpolation)原理及使用三分糖-CSDN博客线性插值法

(2条消息) 降采样,下采样,池化Jiashilin-CSDN博客降采样

c++中冒号（:）和双冒号（::）的用法 - SegmentFault 思否

(2条消息) ROS std_msgs/Header 数据含义_liang_yy的博客-CSDN博客

(2条消息) SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别_微鉴道长的博客-CSDN博客