准备工作
- 引用头文件
// 引用自定义的函数 #include "utility.h" // 自定义的消息类型 #include "lio_sam/cloud_info.h"
- 平滑度结构体、比较平滑度大小
// 定义平滑度结构体 值 与 索引 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; } };
- FeatureExtraction类中变量定义
// 定义特征提取类，继承一个参数服务器 class FeatureExtraction : public ParamServer { public: // 接听CloudInfo信息 ros::Subscriber subLaserCloudInfo; // 发布LaserCloudInfo信息，角点与面片点 ros::Publisher pubLaserCloudInfo; ros::Publisher pubCornerPoints; ros::Publisher pubSurfacePoints; // 提取的点云，角点与面片点 PointType = pcl::PointXYZI pcl::PointCloud<PointType>::Ptr extractedCloud; pcl::PointCloud<PointType>::Ptr cornerCloud; pcl::PointCloud<PointType>::Ptr surfaceCloud; // 下采样 pcl::VoxelGrid<PointType> downSizeFilter; // cloudInfo变量 lio_sam::cloud_info cloudInfo; std_msgs::Header cloudHeader; // 平滑度变量 std::vector<smoothness_t> cloudSmoothness; float *cloudCurvature; int *cloudNeighborPicked; // 近邻点是否已经被标记为特征点 int *cloudLabel; // 点的类型
- 构造函数
FeatureExtraction() { // 接听cloud_info消息（去除畸变处理后） subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay()); // 发布特征提取后的cloud_info消息 角点与平面点 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]; }
点云数据处理
- 主要处理流程
/** * @brief 主要的处理程序部分 * 1、订阅imageProjection节点传入的cloud_info信息，得到header与点云数据（转换成PCL格式） * 2、 * @param msgIn */ void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn) { cloudInfo = *msgIn; // new cloud info cloudHeader = msgIn->header; // new cloud header // 把提取出来的有效的点转成pcl的格式 pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction // 针对上一个节点提取出的有效点（extractedCloud），计算曲率， calculateSmoothness(); // 标记被遮挡的点 与 与激光束平行的点 markOccludedPoints(); // 提取特征点 extractFeatures(); publishFeatureCloud(); }
- 计算每个激光点的曲率（平滑度）值
/** * @brief 计算曲率 * 计算每个点的曲率并存放在cloudCurvature与cloudSmoothness变量中； * 初始化近邻点是否被选中成为标记点、当前点类别标记等标志位变量 */ void calculateSmoothness() { int cloudSize = extractedCloud->points.size(); for (int i = 5; i < cloudSize - 5; i++) { // 计算当前点和周围十个点的距离差 用的距离，而不是x,y,z 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; } }
- 标记两种类型的无效点（被遮挡的点、与激光束几乎平行的点）
判断方式
1）被遮挡的点：两个列号接近的点（列号相差10），深度距离差值比较大（大于0.3m），则认为距离远的点被遮挡。
2) 与激光束平行的点：如果一个点与其前后两个点之间的距离差值较大（大于0.02*当前点到激光雷达中心距离）则认为其余激光束平行。
/** * @brief 去除无效点 * 标记一下遮挡的点 * 标记与激光束平行的点 （后续就不使用这些点作为特征点了） */ 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]; // 计算两个有效点之间的列id差 （距离图像中的列号） int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i])); // 只有比较靠近才有意义 （被遮挡） if (columnDiff < 10){ // 10 pixel diff in range image // 这样depth1容易被遮挡，因此其之前的5个点走设置为无效点 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; } }
- 提取角点与面片点
/** * @brief 提取特征 * 1、遍历所有的扫描线，每一扫描线划分为6份 * 2、提取其中的角点与面片点，其中角点存放在cornerCloud变量中 * 3、除了角点之外的剩余点都放在surfaceCloudScan变量中，进行下采样，放在面片点变量surfaceCloud中 */ void extractFeatures() { // 存储角点 面片点 两类特征点 cornerCloud->clear(); surfaceCloud->clear(); // 平面点 平面点下采样（非特征点，除了前面标记的角点点之外的所有点都放在了这个集合中） pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>()); // 进行下采样，下采样后的点放在了surfaceCloud变量中 pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>()); // 循环所有的扫描线 for (int i = 0; i < N_SCAN; i++) { surfaceCloudScan->clear(); // 把每一根scan等分成6份，每份分别提取特征点 for (int j = 0; j < 6; j++) { // 根据之前得到的每个scan的起始和结束id来均分成6份 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--) { // 找到这个点对应的原先的索引（因为平滑度已经进行过排序，所以这里的k与ind的值不同） int ind = cloudSmoothness[k].ind; // 如果没有被认为是遮挡点或平行点 且曲率大于边缘点阈值（默认为1） if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold) { largestPickedNum++; // 每段最多找20个角点 if (largestPickedNum <= 20){ // 标签置1表示是角点 cloudLabel[ind] = 1; // 这个点收集进存储角点的点云中 cornerCloud->push_back(extractedCloud->points[ind]); } else { break; } // 将这个点周围的几个点设置成遮挡点，避免选取太集中（正方向5个点） cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; l++) { int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1])); // 列idx距离太远就算了，空间上也不会太集中 if (columnDiff > 10) break; cloudNeighborPicked[ind + l] = 1; } // 同理，避免选取太集中（负方向5个点） 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) { // -1表示是面点 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++) { // 注意这里是小于等于0,也就是说不是角点的都认为是面点了 if (cloudLabel[k] <= 0){ surfaceCloudScan->push_back(extractedCloud->points[k]); } } } surfaceCloudScanDS->clear(); // 因为面点太多了，所以做一个下采样 downSizeFilter.setInputCloud(surfaceCloudScan); downSizeFilter.filter(*surfaceCloudScanDS); // 下采样后的面片点存储到surfaceCloud变量中 *surfaceCloud += *surfaceCloudScanDS; } }
- 释放空间、发布提取的特征点
// 将一些不会用到的存储空间释放掉（下一个节点不会用到） void freeCloudInfoMemory() { cloudInfo.startRingIndex.clear(); cloudInfo.endRingIndex.clear(); cloudInfo.pointColInd.clear(); cloudInfo.pointRange.clear(); } /** * @brief 发布相关消息 * 释放空间 * 发送角点 面片点 点云信息集合 */ void publishFeatureCloud() { // free cloud info memory 下一个节点不会用到的信息，释放空间 freeCloudInfoMemory(); // save newly extracted features // 把角点和面点发送给后端 发布提取的特征 cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame); cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame); // publish to mapOptimization 发布给后端优化？ pubLaserCloudInfo.publish(cloudInfo); }
Main函数
初始化一个FeatureExtraction 类，并执行特征提取。
int main(int argc, char** argv) { ros::init(argc, argv, "lio_sam"); /** * @brief 接收经过去畸变预处理后的lio_sam/deskew/cloud_info信息 * 提取 角点与平面点 两类特征点 * 发布lio_sam/feature/cloud_corner与lio_sam/feature/cloud_surface两类消息 * 发布"lio_sam/feature/cloud_info"消息，存储两类特征点 */ FeatureExtraction FE; ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m"); // 单线程处理 ros::spin(); return 0; }
