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 *cloudLabel int *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 points
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points
float 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 image
if (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 beam
float 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 memory
freeCloudInfoMemory();
// save newly extracted features
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, "base_link");
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");
// publish to mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}
ZHOU3 ZHOU4
三、问题汇总及解答
说明：
本来想一个一个处理完成，但是结果发现这些问题自己能够解决的了了无几，因为这几个问题中除了我能解决的之外，剩下的并不与代码的理解相关，还设计到另外的包的内容，毕竟作者是从前边的包的基础上研发的这个系统，因此学习前边的相关的内容的包也是有必要的。如果弄懂了前几个包的内容，说不定后边的这些问题可能就会迎刃而解了。所以我决定先把这些问题能搁置的搁置，先去阅读一下前边的包，并与队友们讨论一下再来处理这些问题。
问题1
答：这是一个基础问题，主要是涉及到cpp语法和点云这一基本结构。
::是域运算符，用来强调声明的是在lvi_sam和std_msgs包下的数据类型点云信息和Header。
cloud_info顾名思义是用来存储点云信息的，而Header则是std_msgs包下用来传递数据的点云信息。
四、参考资料：
LVI-SAM代码注释 – xuwuzhou的SLAM之路 – 静心，慎思，明辨，笃学
(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 思否
标签：odometry,3DSLAM,pointRange,浅析,int,pcl,点云,cloudInfo,cloudNeighborPicked 来源： https://blog.csdn.net/m0_49088083/article/details/120807992