源码阅读，能力有限，如有某处理解错误，请指出，谢谢。
gen_BALM_feature.hpp：主要实现的功能是对于点云计算曲率，并根据曲率来提取特征点，并分为角点和面点。
#include <unordered_set>：无序容器。
// #include "BALM.hpp"
#include <chrono>
#include <fstream>
#include <iostream>
#include <iterator>
#include <string>
#include <time.h>
#include <unordered_set>//无序容器
#include <vector>
#include <eigen3/Eigen/Dense>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// #include <pcl/visualization/pcl_visualizer.h>
// #include <pcl/visualization/cloud_viewer.h>
#include "common/Lidar_parser_base.h"
#define LIDAR_LASER_NUM 64//定义激光雷达线束
#define MAX_POINT_CLOUD_NUM 400000
#define SCAN_LINE_CUT 30//扫描线切割
#define INTENSITY_THRESHOLD 35
float cloudCurvature[MAX_POINT_CLOUD_NUM];//定义云曲率
int cloudSortInd[MAX_POINT_CLOUD_NUM];//定义云杉
int cloudNeighborPicked[MAX_POINT_CLOUD_NUM];//定义被挑选的点云邻域
int cloudLabel[MAX_POINT_CLOUD_NUM];//定义点云标签
bool comp(int i, int j) { return (cloudCurvature[i] < cloudCurvature[j]); }
typedef pcl::PointXYZI PointType;
bool genPcdFeature(pcl::PointCloud<LidarPointXYZIRT>::Ptr laserCloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr pcd_surf,
pcl::PointCloud<pcl::PointXYZI>::Ptr pcd_surf_sharp,
pcl::PointCloud<pcl::PointXYZI>::Ptr pcd_corn) {
int cloud_size = laserCloud->points.size();
if (cloud_size >= MAX_POINT_CLOUD_NUM) {
std::cerr << "[ERROR]too many points in pcd.\n";
return false;
}
// std::cout << "Point Size: " << cloud_size << std::endl;
// pcl::PointCloud<PointType>::Ptr laserCloud(
// new pcl::PointCloud<PointType>());
//vector初始化，得到数量为LIDAR_LASER_NUM，值为0的数据
std::vector<int> scan_start_index(LIDAR_LASER_NUM, 0);
std::vector<int> scan_end_index(LIDAR_LASER_NUM, 0);
// get laser index
std::vector<std::vector<int>> laser_index(LIDAR_LASER_NUM,
std::vector<int>());
for (int i = 0; i < cloud_size; i++) {
int ring = laserCloud->points[i].ring;
//判断激光雷达的线数是否为64线
if (ring < 0 || ring >= LIDAR_LASER_NUM) {
std::cerr << "[ERROR] Wrong Ring value " << ring << " \n";
return false;
}
//把符合线束要求的一帧点云进行数据储存
laser_index[ring].push_back(i);
}
int scan_idx = 0;
//这个for循环实现的功能：对每帧点云的第几次扫描线中的第几个点计算曲率，方法是，同一条扫描线上取目标点左右则各5个点，求均值。并与之作差。
//前5个点和后5个点都无法计算曲率，因为他们不满足左右两侧各有5个点
for (int i = 0; i < LIDAR_LASER_NUM; i++) {
int cur_point_num = laser_index[i].size();
//判断激光雷达扫描中的每线扫描的激光雷达的点云是否满足要求
if (cur_point_num < 11) {
// std::cerr << "[Warning] not enough point on laser scan " << i << ".\n";
scan_idx += cur_point_num;
continue;
}
scan_start_index[i] = scan_idx + 5;
// for(int j = 5; j < cur_point_num - 5; j ++){
for (int j = 0; j < cur_point_num; j++) {
int real_pidx = laser_index[i][j];
int cur_scan_idx = scan_idx + j;
if (j >= 5 && j < cur_point_num - 5) {
float diffX = laserCloud->points[laser_index[i][j - 5]].x +
laserCloud->points[laser_index[i][j - 4]].x +
laserCloud->points[laser_index[i][j - 3]].x +
laserCloud->points[laser_index[i][j - 2]].x +
laserCloud->points[laser_index[i][j - 1]].x -
10 * laserCloud->points[real_pidx].x +
laserCloud->points[laser_index[i][j + 1]].x +
laserCloud->points[laser_index[i][j + 2]].x +
laserCloud->points[laser_index[i][j + 3]].x +
laserCloud->points[laser_index[i][j + 4]].x +
laserCloud->points[laser_index[i][j + 5]].x;
float diffY = laserCloud->points[laser_index[i][j - 5]].y +
laserCloud->points[laser_index[i][j - 4]].y +
laserCloud->points[laser_index[i][j - 3]].y +
laserCloud->points[laser_index[i][j - 2]].y +
laserCloud->points[laser_index[i][j - 1]].y -
10 * laserCloud->points[real_pidx].y +
laserCloud->points[laser_index[i][j + 1]].y +
laserCloud->points[laser_index[i][j + 2]].y +
laserCloud->points[laser_index[i][j + 3]].y +
laserCloud->points[laser_index[i][j + 4]].y +
laserCloud->points[laser_index[i][j + 5]].y;
float diffZ = laserCloud->points[laser_index[i][j - 5]].z +
laserCloud->points[laser_index[i][j - 4]].z +
laserCloud->points[laser_index[i][j - 3]].z +
laserCloud->points[laser_index[i][j - 2]].z +
laserCloud->points[laser_index[i][j - 1]].z -
10 * laserCloud->points[real_pidx].z +
laserCloud->points[laser_index[i][j + 1]].z +
laserCloud->points[laser_index[i][j + 2]].z +
laserCloud->points[laser_index[i][j + 3]].z +
laserCloud->points[laser_index[i][j + 4]].z +
laserCloud->points[laser_index[i][j + 5]].z;
cloudCurvature[real_pidx] =
diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudNeighborPicked[real_pidx] = 0;
cloudLabel[real_pidx] = 0;
}
cloudSortInd[cur_scan_idx] = real_pidx;
}
scan_idx += cur_point_num;
scan_end_index[i] = scan_idx - 6;
}
pcl::PointCloud<PointType> cornerPointsSharp;//极大边线点
pcl::PointCloud<PointType> cornerPointsLessSharp;//次极大边线点
pcl::PointCloud<PointType> surfPointsFlat;//极小平面点
pcl::PointCloud<PointType> surfPointsLessFlat;//次极小平面点（经过降采样）
//这个for循环实现的是提取特征点，提取特征需要注意几条原则：
1、为了提高效率，论文中对每条扫描线分成了4个扇形，而代码中实际是分成了6份，在每份中寻找曲率最大的20个点最为极大角点，对提取数据做了约束，最大点不大于2个，极小面点不大于4个，剩下的编辑未次极小面点；
2、为防止特征点过于集中，每提取一个特征点，就对该点和它附近的点的标志位设置未“已选中”，在循环提取时会跳过已提取的特征点，对于次极小面点采取下采样方式避免特征点扎堆。
for (int i = 0; i < LIDAR_LASER_NUM; i++) {
int valid_scan_point_num = scan_end_index[i] - scan_start_index[i];
// 如果最后一个可算曲率的点与第一个的差小于6，说明无法分成6个扇区，跳过
if (valid_scan_point_num < 6)
continue;
// std::cout << "valid scan " << i << " : " << valid_scan_point_num <<
// std::endl;
// 用来存储不太平整的点
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(
new pcl::PointCloud<PointType>);
// 将每个scan等分成SCAN_LINE_CUT等分
for (int j = 0; j < SCAN_LINE_CUT; j++) {
// 每个等分的起始和结束点
int sp = scan_start_index[i] + valid_scan_point_num * j / SCAN_LINE_CUT;
int ep = scan_start_index[i] +
valid_scan_point_num * (j + 1) / SCAN_LINE_CUT - 1;
int sp_scan = valid_scan_point_num * j / SCAN_LINE_CUT + 5;
int ep_scan = valid_scan_point_num * (j + 1) / SCAN_LINE_CUT - 1 + 5;
// TicToc t_tmp;
// 对点云按照曲率进行排序，小的在前，大的在后
std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);
// t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
// 从最大曲率往最小曲率遍历，寻找边线点，并要求大于0.1
for (int k = ep; k >= sp; k--) {
// 排序后顺序就乱了，这个时候索引的作用就体现出来了
// 取出点的索引
int ind = cloudSortInd[k];
//判断所取点的强度是否达到强度阈值
if (laserCloud->points[ind].intensity < INTENSITY_THRESHOLD)
continue;
std::vector<int>::iterator pos_loc =
std::find(laser_index[i].begin() + sp_scan,
laser_index[i].begin() + ep_scan, ind);
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 10) { // 0.1
largestPickedNum++;
// 每段选2个曲率大的点
if (largestPickedNum <= 2) {
// label为2是曲率大的标记
cloudLabel[ind] = 2;
//实例化对象pt，进行复制传递
PointType pt;
pt.x = laserCloud->points[ind].x;
pt.y = laserCloud->points[ind].y;
pt.z = laserCloud->points[ind].z;
pt.intensity = laserCloud->points[ind].intensity;
// cornerPointsSharp存放大曲率的点
// 既放入极大边线点容器，也放入次极大边线点容器
cornerPointsSharp.push_back(pt);
cornerPointsLessSharp.push_back(pt);
}
// 以及20个曲率稍微大一些的点
else if (largestPickedNum <= 20) { // 20
// label置1表示曲率稍微大
// 超过2个选择点以后，设置为次极大边线点，仅放入次极大边线点容器
cloudLabel[ind] = 1;
//实例化对象pt，进行复制传递
PointType pt;
pt.x = laserCloud->points[ind].x;
pt.y = laserCloud->points[ind].y;
pt.z = laserCloud->points[ind].z;
pt.intensity = laserCloud->points[ind].intensity;
cornerPointsLessSharp.push_back(pt);
}
// 超过20个就算了
else {
break;
}
// 这个点被选中后 pick标志位置1
cloudNeighborPicked[ind] = 1;
// 为了保证特征点不过度集中，将选中的点周围5个点都置1,避免后续会选到
// ID为ind的特征点的相邻scan点距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布
// 右侧
for (int l = 1; l <= 5; l++) {
// 查看相邻点距离是否差异过大，如果差异过大说明点云在此不连续，是特征边缘，就会是新的特征，因此就不置位了
float diffX = laserCloud->points[*(pos_loc + l)].x -
laserCloud->points[*(pos_loc + l - 1)].x;
float diffY = laserCloud->points[*(pos_loc + l)].y -
laserCloud->points[*(pos_loc + l - 1)].y;
float diffZ = laserCloud->points[*(pos_loc + l)].z -
laserCloud->points[*(pos_loc + l - 1)].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[*(pos_loc + l)] = 1;
}
//左侧 同理
for (int l = -1; l >= -5; l--) {
float diffX = laserCloud->points[*(pos_loc + l)].x -
laserCloud->points[*(pos_loc + l + 1)].x;
float diffY = laserCloud->points[*(pos_loc + l)].y -
laserCloud->points[*(pos_loc + l + 1)].y;
float diffZ = laserCloud->points[*(pos_loc + l)].z -
laserCloud->points[*(pos_loc + l + 1)].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[*(pos_loc + l)] = 1;
}
}
}
//下面开始挑选面点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSortInd[k];
// 确保这个点没有被pick且曲率小于阈值
if (laserCloud->points[ind].intensity < INTENSITY_THRESHOLD)
continue;
std::vector<int>::iterator pos_loc =
std::find(laser_index[i].begin() + sp_scan,
laser_index[i].begin() + ep_scan, ind);
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 10) {
// -1认为是平坦的点
cloudLabel[ind] = -1;
PointType pt;
pt.x = laserCloud->points[ind].x;
pt.y = laserCloud->points[ind].y;
pt.z = laserCloud->points[ind].z;
pt.intensity = laserCloud->points[ind].intensity;
surfPointsFlat.push_back(pt);
smallestPickedNum++;
// 这里不区分平坦和比较平坦，因为剩下的点label默认是0,就是比较平坦
if (smallestPickedNum >= 4) {
break;
}
// 同样距离 < 0.05的点全设为已经选择过
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
float diffX = laserCloud->points[*(pos_loc + l)].x -
laserCloud->points[*(pos_loc + l - 1)].x;
float diffY = laserCloud->points[*(pos_loc + l)].y -
laserCloud->points[*(pos_loc + l - 1)].y;
float diffZ = laserCloud->points[*(pos_loc + l)].z -
laserCloud->points[*(pos_loc + l - 1)].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[*(pos_loc + l)] = 1;
}
for (int l = -1; l >= -5; l--) {
float diffX = laserCloud->points[*(pos_loc + l)].x -
laserCloud->points[*(pos_loc + l + 1)].x;
float diffY = laserCloud->points[*(pos_loc + l)].y -
laserCloud->points[*(pos_loc + l + 1)].y;
float diffZ = laserCloud->points[*(pos_loc + l)].z -
laserCloud->points[*(pos_loc + l + 1)].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[*(pos_loc + l)] = 1;
}
}
}
// 选取次极小平面点，除了极大平面点、次极大平面点，剩下的都是次极小平面点
for (int k = sp; k <= ep; k++) {
int ind = cloudSortInd[k];
if (laserCloud->points[ind].intensity < INTENSITY_THRESHOLD)
continue;
// 这里可以看到，剩下来的点都是一般平坦，这个也符合实际
if (cloudLabel[ind] <= 0) {
PointType pt;
pt.x = laserCloud->points[ind].x;
pt.y = laserCloud->points[ind].y;
pt.z = laserCloud->points[ind].z;
pt.intensity = laserCloud->points[ind].intensity;
surfPointsLessFlatScan->push_back(pt);
}
}
}
// 对每一条scan线上的次极小平面点进行一次降采样
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
// 一般平坦的点比较多，所以这里做一个体素滤波
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
// surfPointsLessFlat += *surfPointsLessFlatScan;
}
// std::cout << "surfPointsFlat point size: " << surfPointsFlat.points.size()
// << std::endl;
// std::cout << "surfPointsLessFlat point size: " <<
// surfPointsLessFlat.points.size() << std::endl;
// std::cout << "cornerPointsSharp point size: " <<
// cornerPointsSharp.points.size() << std::endl;
// std::cout << "cornerPointsLessSharp point size: " <<
// cornerPointsLessSharp.points.size() << std::endl;
// pcl::PCDWriter writer;
// writer.write("surfPointsFlat.pcd", surfPointsFlat);
// writer.write("cornerPointsSharp.pcd", cornerPointsSharp);
// writer.write("cornerPointsLessSharp.pcd", cornerPointsLessSharp);
// writer.write("surfPointsLessFlat.pcd", surfPointsLessFlat);
*pcd_surf = surfPointsLessFlat;
*pcd_surf_sharp = surfPointsFlat;
*pcd_corn = cornerPointsSharp;
surfPointsFlat.clear();
surfPointsLessFlat.clear();
cornerPointsSharp.clear();
cornerPointsLessSharp.clear();
return true;
}
// int main(int argc, char **argv) {
// pcl::PointCloud<LidarPointXYZIRT>::Ptr cloud(
// new pcl::PointCloud<LidarPointXYZIRT>);
// std::string lidar_file_name = "haha.pcd";
// if (pcl::io::loadPCDFile(lidar_file_name, *cloud) < 0) {
// std::cout << "cannot open pcd_file: " << lidar_file_name << "\n";
// exit(1);
// }
// pcl::PointCloud<pcl::PointXYZI>::Ptr pl_corn(new
// pcl::PointCloud<pcl::PointXYZI>);
// pcl::PointCloud<pcl::PointXYZI>::Ptr pl_surf(new
// pcl::PointCloud<pcl::PointXYZI>);
// genPcdFeature(cloud, pl_surf, pl_corn);
// return 0;
// }参考链接：
6847
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
