对于当下多传感器融合框架,LVI-SAM、R2LIVE和R3LIVE成为团队研究的热点和重点,今天写一下featureExtraction.cpp的代码阅读。
目录
头文件
#include "utility.h" // 包含了本系统中所需要的各种初始定义,位于本地文件夹下
#include "lvi_sam/cloud_info.h" // 点云的基本信息,也包含了imu 和 odom的成员变量
这个头文件包括了两个,一个是utility,另一个是cloud_info,这个在后面用的比较多,它是作者团队自己编写的。
结构体
struct smoothness_t
{
float value;
size_t ind;
};
struct by_value
{
bool operator()(smoothness_t const &left, smoothness_t const &right)
{
return left.value < right.value;
}
};
使用两个结构体,第一个是用来表示平滑参数的(看的其他的文章,这里的smoothness有这么个翻译的方式)。另一个使用在sort函数里面作为第三个参数进行传递的,就是返回从小到大的排序
类
它继承了ParamServer类,下面说一下它的输入和输出,还有一些变量
ros::Subscriber subLaserCloudInfo; // 输入:点云信息
// 输出:点云信息
ros::Publisher pubLaserCloudInfo; // 用来完成发布不同的点云信息,激光帧提取特征之后的点云信息
ros::Publisher pubCornerPoints; // 激光帧提取到的角点点云
ros::Publisher pubSurfacePoints; // 激光帧提取到的平面点点云
pcl::PointCloud<PointType>::Ptr extractedCloud; // pcl格式的提取点云 、 角点 、 平面点
pcl::PointCloud<PointType>::Ptr cornerCloud;
pcl::PointCloud<PointType>::Ptr surfaceCloud;
pcl::VoxelGrid<PointType> downSizeFilter;
//用来存储当前激光帧的全部信息,包括所有的历史数据
lvi_sam::cloud_info cloudInfo;
std_msgs::Header cloudHeader; // Header是std_msgs包下用来传递数据的点云信息
//用来存储激光帧点云的曲率
std::vector<smoothness_t> cloudSmoothness; // 平滑度 这个待确定
float *cloudCurvature; // 曲率
//有待确定,一个用来表示是否提取,一个用来表示提取的类型:角点还是平面点
int *cloudNeighborPicked;
int *cloudLabel;
上面的变量不详细地解释,ros::表示是在ros作用域下面,pcl::表示是在pcl作用域下面,这个pcl是point cloud library的缩写,就是把点云搞成统一的格式。然后voxelgrid后面的downsizefilter,这个就是体素滤波,就是我把正方体里面的所有点云用它的重心来表示,这样数据量就大大减少。cloudheader里面存储的是头部信息,但是这个没太明白。下面就是平滑度,曲率啥的这个不需要看。但是曲率的计算这个cpp文件里面没看到。然后下面的话cloudNeighborPicked表示这个点是否选取,当值为1的时候说明这个点是遮挡点或者平行光束点,意思就是不再用这个点了。然后cloudLabel这个值,当它为1的时候,判定为角点。当它的值为-1的时候,判定为平面点。这个是从下面的函数里面看出来的,具体哪个函数,下面进行解释。
//构造函数,用来初始化
//它们初始化了订阅者和发布者的信息,并且给上文所定义的各个变量进行初始化。
FeatureExtraction()
{
//订阅当前激光帧运动畸变校正后的点云信息
subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/deskew/cloud_info", 5, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// ros::Subscriber subLaserCloudInfo; 它的定义是在ros作用域下面的,那么它获取的数据最后需要在ros里面进行展示
//发布当前激光帧提取特征之后的点云信息
pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5);
//pubLaserCloudInfo = nh.advertice<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5); 后面是发布的路径 sub订阅 pub发布 5是传给订阅者的最大传出信息数
//发布当前激光帧的角点点云
pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_corner", 5);
//发布当前激光帧的平面点点云
pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_surface", 5);
//调用初始化函数
initializationValue();
}
上面是构造函数,第一行订阅的时候,用到了handler函数,这个在下面进行解释,然后就是这个文件的输入输出,pub就是输出,sub就是输入。然后最后初始化。接下来讲一下初始化。
// 初始化各个变量信息
// 平滑度重新设定大小
// 体素滤波的小方块重新设置大小
// 提取点云 角点 平面点重新设置
// 曲率 、 周围点是否被提取 、 是不是角点label这些内容也重新构建数组
// 3.23 HuangY
void initializationValue()
{
cloudSmoothness.resize(N_SCAN*Horizon_SCAN); // 这个resize就是重新划定大小的,这个SCAN一看就知道是雷达的扫描线
// 体素滤波:就是简单理解成 我放一个正方体 处在这个正方体里面的点云用正方体的重心来表示即可 这样实现了降采样
downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize); // 设置体素大小的
extractedCloud.reset(new pcl::PointCloud<PointType>()); // 这个reset函数就是重新设置里面的参数,括号里面的内容就是前面变量的类型,这个对齐即可
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]; // 这个我认为是判断点云的周围几个点有没有被提取,如果使用过了(也就是提取过了)赋值为1, 没使用过就为0
cloudLabel = new int[N_SCAN*Horizon_SCAN]; // 这个表示这个点是否是角点的
}
这个就是初始化,里面的步骤在上面就已经详细地解释了,就是先初始化各个变量信息,然后重新设置大小,然后各种操作。
void laserCloudInfoHandler(const lvi_sam::cloud_infoConstPtr& msgIn) // 在构造函数的第一个sub变量中使用,handler是个句柄,但是看见某些文章也说它是回调函数
{
// lvi_sam作用域下的点云信息mgsIn
cloudInfo = *msgIn; // new cloud info 生成一个新的点云
cloudHeader = msgIn->header; // new cloud header 在这里msgIn是一个引用,所以其成员header需要用->来表示
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction 提取出来有效点云,deskewed去歪斜
calculateSmoothness(); // 直接函数名的意思,计算平滑度,看下面的函数主体
markOccludedPoints();
extractFeatures();
publishFeatureCloud();
}
这个函数就是我说的构造函数的第一行里面使用的参数里面的函数,这个的话就是下面有四个函数,这些函数就构成了这个cpp的功能,进行特征提取。对于这个特征提取的话,我现在的理解就是先计算平滑参数,然后标记遮挡点和平行光束点(因为这两类点影响雷达点云)。然后提取特征,就是提取角点和平面点,最后发布信息。
// 这个地方就是在计算点的平滑参数的,要细看lio-sam
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size(); // 先获取点云的大小
//遍历所有的有效点云(extractedCloud)
for (int i = 5; i < cloudSize - 5; i++)
{
//用当前激光点前后5个点计算当前点的曲率,因为是计算前后5个点云,所以在这里i从5开始
// diffRange是这个点的pointRange 和 前后5个点的pointRange的差值
// 这里的pointRange还不是很明白,可能是点云的深度???点的范围,我觉得可能是深度,毕竟这里是float浮点数,肯定不是个范围,是个数值 3.25
// 平滑性采用的方法为将当前点的距离与前后各5点计算误差累计和;显然误差累计和较大,则此点属于跳跃点,或边缘点; 通过这里,这个部分还得看lio-sam里面的内容
// 这里的pointRange,表示深度,在下面的函数里面,它赋值给了深度! 3.25解决
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;
}
}
这个就是计算平滑参数的,这里有个疑问,就是这个一维索引来存储点云到底是怎么存储的???感觉很迷。然后这里的pointRange的理解也不是很好,我通过下面的代码,发现是深度。但是在判断平行光束的时候,要是深度的话。。。我不理解怎么判断了。。。还有这个curvature的计算,也是很迷,我不明白为什么是相加再相乘以后就直接dx*dx + dy*dy +dz*dz了
//用来标记两种不同的点,分别是被遮挡的 和 被标记的。
void markOccludedPoints() // Occluded 遮挡的
{
int cloudSize = extractedCloud->points.size(); // 通用操作,先获取大小
// mark occluded points and parallel beam points 标记遮挡点 和 平行光束点
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points 这部分来标记遮挡点,从i = 5就知道要前后5个来进行操作。这里对为什么是前后5个 而不是6个 7个表示疑问 3.25 还是得读论文lio-sam
float depth1 = cloudInfo.pointRange[i]; // 这里的pointRange 推测,应该就是深度
float depth2 = cloudInfo.pointRange[i+1];
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i])); // 这里用pointColInd来计算columnDiff,然后进入下面的判断
if (columnDiff < 10)
{
// 10 pixel diff in range image
// 标记成1的 就是屏蔽的点
// 参考地址:https://www.daimajiaoliu.com/daima/4ed329fe7900408
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
// pointRange就是深度值,对于一维数组的索引的话 可以是左右 也可以是前后 这个问题暂留
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
// 平行光束,即激光打上去之后,物体是和激光在统一平面上,即它的深度无法知道,这个就叫平行光束
// 这里的判断条件,就是看第i个点,左右两个i-1 和 i+1他们之间的左右距离进行判断
// 感觉这里的pointRange可以是深度,就是它的范围值,来计算的。这个pointRange暂时没搞清楚
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
// 到这里基本结束这个遮挡点 和 平行光束点的判断。赋值为1的NeighborPicked感觉后面应该是不会使用的,再进行验证 3.25
}
}
这里的话就是来标记遮挡点和平行光束点的。上面注释的很详细不过多展开。
//特征提取,最核心的函数
void extractFeatures()
{
//首先清除原来的信息,并创建指针,用来存放平面点 和 降采样以后的平面点
cornerCloud->clear();
surfaceCloud->clear();
// clear()就是清楚里面的变量的值,并且resize一下。
// cornerCloud->clear();
// surfaceCloud->clear();
pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>()); // 平面点云扫描
pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>()); // 平面点云扫描 然后DownSample一下
for (int i = 0; i < N_SCAN; i++)
{
//N_SCAN 是在utility.h中定义的变量,这个是用来存储雷达中的数据:扫描线
//因此第一个for循环的作用是遍历所有的扫描线。
surfaceCloudScan->clear(); // 先清除surfaceCloudScan里面的值
for (int j = 0; j < 6; j++)
{
//这里的6是一个常量,是作者用来分段的数目,将每一个SCAN分为六段来进行分析
//用线性插值对SCAN进行等分,取得sp和ep
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; // 就结束下面的内容,然后j++。所以需要sp < ep 才能执行下面的内容
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value()); // 排序 by_value()表示这个sort需要从小到大进行排序
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)
{
int ind = cloudSmoothness[k].ind; // 是一个标签,为了避免重复访问点云,就是index
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
//判断条件:当前激光点还未被处理,且曲率大于边的阈值,则认为是角点 neighborpicked见上面的那个函数,等于0是好的点,等于1是遮挡点和平行光束点中的一个
//这里的edgeThreshold默认值为0.1
largestPickedNum++;
if (largestPickedNum <= 20)
{
//在每一段中只提取出来20个点,然后将其标记为角点,加入角点点云
cloudLabel[ind] = 1; // 推测这里的cloudLabel = 1就表示这个点是角点
cornerCloud->push_back(extractedCloud->points[ind]);
// 还有这里的push_back()函数有专门地写它,可以自行转到定义去看
// cornerCloud->push_back(extractedCloud->points[ind]);
// 这里的变化的量是ind,是点的index
}
else
{
break;
}
cloudNeighborPicked[ind] = 1; // 在这里 这个点经过上面的if以后,进行赋值,让它 == 1,不再用它了
for (int l = 1; l <= 5; l++)
{
//同一条扫描线上前5个点标记一下,不再处理,避免特征聚集
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
if (columnDiff > 10) // 这个10 在上面的遮挡点的if里面 可以去看
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前后5个点,然后是不是遮挡点的,因为根据里面的if的条件,初步感觉是在判断遮挡点的,如果没有break,则赋值为1,和上面的遮挡点判断的那个函数效果相同
}
}
for (int k = sp; k <= ep; k++)
{
int ind = cloudSmoothness[k].ind; // 提取index
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
{
// 判断条件:当前激光点还未被处理,且曲率小于平面阈值,这里的surfThreshold应该也是0.1
cloudLabel[ind] = -1; // 不是角点,赋值成-1,根据293行的if条件,可以知道,这个为平面点,平面点:赋值成-1
cloudNeighborPicked[ind] = 1; // 然后这个neighborpicked赋值成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和上面的两个for效果一样,就是在判断是否是遮挡点,然后赋值成1,就不再进行操作了
}
}
// 这个for循环是用来进行平面点云的添加的
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
// 总结:角点:ep -> sp进行循环 从大到小
// 平面点:sp -> ep进行循环 从小到大 然后if条件里面的参数要看清楚
}
// 一个降采样的操作,把surfaceCloudScan -> surfaceCloudScanDS -> surfaceCLoud
surfaceCloudScanDS->clear();
downSizeFilter.setInputCloud(surfaceCloudScan); // 降采样
downSizeFilter.filter(*surfaceCloudScanDS);
*surfaceCloud += *surfaceCloudScanDS;
}
}
然后这个就是featureExtraction的主要部分,就是提取角点和平面点的。这里不明白的是sp和ep的取值方法,看到别人说的是线性插值,可能还需要回归lio-sam论文的阅读了。然后对平滑参数进行排序,这里的平滑参数,好吧,读懂代码了,平滑参数的值他就是曲率,曲率的计算不太明白。好了,这个地方没问题了。然后接着对角点进行判断,从大到小进行for循环。里面的内容不过多展开。然后是平面点的提取,这里使用了两个循环,我觉得是不是可以合成一个呢,因为第三个for循环,他判断cloudlabel<0就push进surfacecloud里面了。最后就是降采样,这三行函数希望自己牢牢记在心里!
void freeCloudInfoMemory() // 清理点云信息
{
// cloudinfo是lvi_sam::作用域下的
// 对startRingIndex endRingIndex pointColInd pointRange进行先clear 再 shrink to fit函数操作
cloudInfo.startRingIndex.clear();
cloudInfo.startRingIndex.shrink_to_fit(); // 这里的shrink_to_fit()函数的作用就是看这个容器的大小,如果你本来装10个的,但是你申请了100个的空间,那么这个函数执行以后,你的空间大小变成了10个
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() // 发布点云信息
{
//先清理点云信息,再保存好新提取到的点云信息,然后传送给图优化函数mapOptimization
// free cloud info memory
freeCloudInfoMemory();
// save newly extracted features 保存了上面提取的特征
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, "base_link"); // 这个肯定就是publishCloud以后返回了一个值
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");
// publish to mapOptimization 这些给了图优化,现在的重中之重,待解决:图优化到底输入了些啥
pubLaserCloudInfo.publish(cloudInfo);
}
这个没啥好说的,解释一下shrink to fit这个就是你申请的空间大了,系统自动给你缩小空间的。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lidar"); // 这个是ros的初始化,但是有时候节点的名字会被launch给覆盖,这个不需要过多纠结
FeatureExtraction FE; // 这个是这个类的实体
ROS_INFO("\033[1;32m----> Lidar Feature Extraction Started.\033[0m"); // 这个是发消息的,使用roslaunch的时候会在terminal里面看见它
ros::spin();
// 这句话的意思是循环且监听反馈函数(callback)。 懂了,这个函数就是写在这里调用callback的,具体哪个还需要再进一步深究
// 循环就是指程序运行到这里,就会一直在这里循环了。监听反馈函数的意思是,如果这个节点有callback函数,那写一句ros::spin()在这里,
// 就可以在有对应消息到来的时候,运行callback函数里面的内容。
// 就目前而言,以我愚见,我觉得写这句话适用于写在程序的末尾(因为写在这句话后面的代码不会被执行),适用于订阅节点,且订阅速度没有限制的情况。
return 0;
}
然后这个主函数的话,一般套路,先ros::init,再创建个类的实体,然后ros上面发布个信息,里面的函数的话就是创建实体的时候运行完了。最后这个ros::spin()也是第一次见,然后查阅完资料以后恍然大悟。
到此这部分结束,对现有的问题进行一个总结:
问题
1、一维索引怎么存储点云的
2、pointRange的理解,平行光束点判断的那个部分
3、曲率的计算
4、发布给图优化的那部分也没太懂