0. 简介
激光雷达作为自动驾驶最常用的传感器，经常需要使用激光雷达来做建图、定位和感知等任务。而这时候使用降低点云规模的预处理方法，可以能够去除无关区域的点以及降低点云规模。并能够给后续的PCL点云分割带来有效的收益。
1. 点云预处理
1.1 指定区域获取点云
在实际使用中，我们可以看出，虽然点云的分布范围较广，但大部分的点都集中的中间区域，距离越远点云越稀疏，相对的信息量也越小。此外还能明显看到一些离群点，因此我们可以筛选掉一些较远的点，只保留我们感兴趣范围内的点。以下为保留 x 在 30m，y 在 15m，z 在 2m 范围内的点的效果：
template <class PointType>
void removePointsOutsideRegion(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
const std::pair<double, double>& x_range,
const std::pair<double, double>& y_range,
const std::pair<double, double>& z_range) {
int num_points = src_cloud_ptr->points.size();
boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
cloud_ptr->points.reserve(num_points);
for (const auto& pt : src_cloud_ptr->points) {
bool inside = (pt.x >= x_range.first && pt.x <= x_range.second && pt.y >= y_range.first &&
pt.y <= y_range.second && pt.z >= z_range.first && pt.z <= z_range.second);
if (inside) {
cloud_ptr->points.push_back(pt);
}
}
dst_cloud_ptr = cloud_ptr;
}
// 或者使用CropBox来实现去除给定区域外的点
pcl::CropBox<pcl::PointXYZ> box_filter;
box_filter.setInputCloud(cloud_ptr);
box_filter.setMin(Eigen::Vector4f(keep_x_range.first, keep_y_range.first, keep_z_range.first, 1.0));
box_filter.setMax(Eigen::Vector4f(keep_x_range.second, keep_y_range.second, keep_z_range.second, 1.0));
box_filter.filter(*temp_cloud_ptr);
1.2 去除给定区域的点
在某些情况下，我们也会需要去除给定区域内部的点，比如在自动驾驶中激光扫描的区域有一部分来自搭载激光雷达的车子本身
template <class PointType>
void filterPointsWithinRegion(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
const std::pair<double, double>& x_range,
const std::pair<double, double>& y_range,
const std::pair<double, double>& z_range,
bool remove) {
int num_points = src_cloud_ptr->points.size();
boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
cloud_ptr->points.reserve(num_points);
for (const auto& pt : src_cloud_ptr->points) {
bool inside = (pt.x >= x_range.first && pt.x <= x_range.second && pt.y >= y_range.first &&
pt.y <= y_range.second && pt.z >= z_range.first && pt.z <= z_range.second);
if (inside ^ remove) {
cloud_ptr->points.push_back(pt);
}
}
dst_cloud_ptr = cloud_ptr;
}
// PassThrough: 可以指定点云中的点的某个字段进行范围限制，将其设为 true 时可以进行给定只保留给定范围内的点的功能
pcl::PassThrough<pcl::PointXYZ> pass_filter;
bool reverse_limits = true;
pass_filter.setInputCloud(filtered_cloud_ptr);
pass_filter.setFilterFieldName("x");
pass_filter.setFilterLimits(-5, 5);
pass_filter.getFilterLimitsNegative(reverse_limits); // reverse the limits
pass_filter.filter(*filtered_cloud_ptr);
pass_filter.setFilterFieldName("y");
pass_filter.setFilterLimits(-2, 2);
pass_filter.getFilterLimitsNegative(reverse_limits); // reverse the limits
pass_filter.filter(*filtered_cloud_ptr);
pass_filter.setFilterFieldName("z");
pass_filter.setFilterLimits(-2, 2);
pass_filter.getFilterLimitsNegative(reverse_limits); // reverse the limits
pass_filter.filter(*filtered_cloud_ptr);
1.3 点云下采样
1.3.1 栅格化采样
这里第一点介绍栅格化的下采样，在 PCL 中对应的函数为体素滤波。栅格化下采样大致的思路是计算整体点云的中心，通过计算每个点到中心的距离结合要求的分辨率计算栅格对应的坐标，并入其中，最后遍历每个包含点的栅格计算其中点的几何中心或者取该栅格中心加入目标点云即可。
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setLeafSize(0.1, 0.1, 0.1);
voxel_filter.setInputCloud(cloud_ptr);
voxel_filter.filter(*filtered_cloud_ptr);
1.3.2 点云所在区域密度规律滤波
该方法直接基于点云分布密度进行去噪，直观的感受是可以根据点云中每个点所在区域判断其是否是噪声，一般来说噪声点所在区域都比较稀疏。
pcl::RadiusOutlierRemoval<pcl::PointXYZ>::Ptr radius_outlier_removal(
new pcl::RadiusOutlierRemoval<pcl::PointXYZ>(true));
radius_outlier_removal->setInputCloud(cloud_ptr);
radius_outlier_removal->setRadiusSearch(1.0);
radius_outlier_removal->setMinNeighborsInRadius(10);
radius_outlier_removal->filter(*filtered_cloud_ptr);
1.3.3 点云所在区域分布规律滤波
除了根据稠密意以外还可以根据距离来筛选滤波，每个点计算其到周围若干点的平均距离，如果这个平均距离相对于整体点云中所有点的平均距离较近，则认为其不是噪点
// PCL built-in radius removal
pcl::StatisticalOutlierRemoval<pcl::PointXYZ>::Ptr statistical_outlier_removal(
new pcl::StatisticalOutlierRemoval<pcl::PointXYZ>(true)); // set to true if we want to extract removed indices
statistical_outlier_removal->setInputCloud(cloud_ptr);
statistical_outlier_removal->setMeanK(20);
statistical_outlier_removal->setStddevMulThresh(2.0);
statistical_outlier_removal->filter(*filtered_cloud_ptr);
1.3.4 根据点云是否可以被稳定观察到筛选
LOAM 中对点云中的点是否能形成可靠特征的一个判断标准是它能否被稳定观察到。LOAM 中着重提了这两种情况的点是不稳定的：
- 特征成平面和扫描线近乎平行
- 特征扫描到的其中一端被另一个平面挡住，这部分的点也不稳定
template <typename PointType>
void filter_occuluded_points(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
int neighbor_count,
float distance_threshold,
float horizontal_angle_diff_threshold,
boost::shared_ptr<std::vector<int>> removed_indices = nullptr) {
int cloud_size = src_cloud_ptr->points.size();
distance_threshold = std::fabs(distance_threshold);
boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
cloud_ptr->points.reserve(cloud_size);
std::vector<int> status(cloud_size, 0);
for (int i = neighbor_count; i < cloud_size - neighbor_count; ++i) {
const PointType& pt1 = src_cloud_ptr->points[i];
const PointType& pt2 = src_cloud_ptr->points[i + 1];
double horizontal_angle_1 = std::atan2(pt1.y, pt1.x) / M_PI * 180.0;
double horizontal_angle_2 = std::atan2(pt2.y, pt2.x) / M_PI * 180.0;
if (std::fabs(horizontal_angle_1 - horizontal_angle_2) > horizontal_angle_diff_threshold) continue;
float range1 = std::sqrt(pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z);
float range2 = std::sqrt(pt2.x * pt2.x + pt2.y * pt2.y + pt2.z * pt2.z);
if (range1 - range2 > distance_threshold) // pt1 is occluded
{
for (int j = i; j >= i - neighbor_count; j--) {
status[j] = 1;
}
} else if (range2 - range1 > distance_threshold) { // pt2 is occluded
for (int j = i + 1; j <= i + neighbor_count; j++) {
status[j] = 1;
}
}
}
for (int i = 0; i < cloud_size; ++i) {
if (status[i] == 0) {
cloud_ptr->points.push_back(src_cloud_ptr->points[i]);
} else if (removed_indices != nullptr) {
removed_indices->push_back(i);
}
}
dst_cloud_ptr = cloud_ptr;
}
2. 特征提取
2.1 激光雷达时间序列
这一帧数据中点的排列顺序为从最高的线束到最低的线束进行排列，每条线束之间点按逆时针的顺序排列。目前大部分主流激光雷达应该都可以直接在点云中提供每个点对应的扫描线已经时间戳，所以其实可以不用这种粗略的方法来进行计算。
template <typename PointType>
void sortPointCloudByScanLine(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>>& dst_cloud_ptr_vec) {
const int N_SCAN = 64;
dst_cloud_ptr_vec.resize(N_SCAN);
dst_cloud_ptr_vec.clear();
PointType pt;
int line_id;
for (int i = 0; i < src_cloud_ptr->points.size(); ++i) {
pt = src_cloud_ptr->points[i];
line_id = 0;
double elevation_angle = std::atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)) / M_PI * 180.0;
// adapt from a-loam
if (elevation_angle >= -8.83)
line_id = int((2 - elevation_angle) * 3.0 + 0.5);
else
line_id = 64 / 2 + int((-8.83 - elevation_angle) * 2.0 + 0.5);
if (elevation_angle > 2 || elevation_angle < -24.33 || line_id > 63 || line_id < 0) {
continue;
}
if (dst_cloud_ptr_vec[line_id] == nullptr)
dst_cloud_ptr_vec[line_id] = boost::make_shared<pcl::PointCloud<PointType>>();
dst_cloud_ptr_vec[line_id]->points.push_back(pt);
}
}
2.2 三维激光雷达压缩成二维
void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const{
ground.header = pc.header;
nonground.header = pc.header;
if (pc.size() < 50){
ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction");
nonground = pc;
} else {
// https://blog.csdn.net/weixin_41552975/article/details/120428619
// 指模型参数，如果是平面的话应该是指a b c d四个参数值
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation<PCLPoint> seg;
//可选设置
seg.setOptimizeCoefficients (true);
//必须设置
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
// 设置迭代次数的上限
seg.setMaxIterations(200);
// 设置距离阈值
seg.setDistanceThreshold (0.04);
//设置所搜索平面垂直的轴
seg.setAxis(Eigen::Vector3f(0,0,1));
//设置待检测的平面模型和上述轴的最大角度
seg.setEpsAngle(0.15);
// pc 赋值
PCLPointCloud cloud_filtered(pc);
//创建滤波器
pcl::ExtractIndices<PCLPoint> extract;
bool groundPlaneFound = false;
while(cloud_filtered.size() > 10 && !groundPlaneFound){
// 所有点云传入，并通过coefficients提取到所有平面
seg.setInputCloud(cloud_filtered.makeShared());
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0){
ROS_INFO("PCL segmentation did not find any plane.");
break;
}
//输入要滤波的点云
extract.setInputCloud(cloud_filtered.makeShared());
//被提取的点的索引集合
extract.setIndices(inliers);
if (std::abs(coefficients->values.at(3)) < 0.07){
ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
//true:滤波结果取反，false，则是取正
extract.setNegative (false);
//获取地面点集合，并传入ground
extract.filter (ground);
// 存在有不是平面的点
if(inliers->indices.size() != cloud_filtered.size()){
extract.setNegative(true);
PCLPointCloud cloud_out;
// 传入cloud_out
extract.filter(cloud_out);
// 不断减少cloud_filtered数目，同时累加nonground数目
cloud_filtered = cloud_out;
nonground += cloud_out;
}
groundPlaneFound = true;
} else{ // 否则提取那些不是平面的，然后剩下的就是平面点
ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
pcl::PointCloud<PCLPoint> cloud_out;
extract.setNegative (false);
extract.filter(cloud_out);
nonground +=cloud_out;
if(inliers->indices.size() != cloud_filtered.size()){
extract.setNegative(true);
cloud_out.points.clear();
extract.filter(cloud_out);
cloud_filtered = cloud_out;
} else{
cloud_filtered.points.clear();
}
}
}
// 由于没有找到平面，则会进入下面
if (!groundPlaneFound){
ROS_WARN("No ground plane found in scan");
// 对高度进行粗略调整，以防止出现虚假障碍物
pcl::PassThrough<PCLPoint> second_pass;
second_pass.setFilterFieldName("z");
second_pass.setFilterLimits(-m_groundFilterPlaneDistance, m_groundFilterPlaneDistance);
second_pass.setInputCloud(pc.makeShared());
second_pass.filter(ground);
second_pass.setFilterLimitsNegative (true);
second_pass.filter(nonground);
}
// Create a set of planar coefficients with X=Y=0,Z=1
pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients());
coefficients1->values.resize(4);
coefficients1->values[0] = 1;
coefficients1->values[1] = 0;
coefficients1->values[2] = 0;
coefficients1->values[3] = 0;
// Create the filtering object
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(nonground);
proj.setModelCoefficients(coefficients1);
proj.filter(*cloud_projected);
if (cloud_projected.size() > 0)
writer.write<PCLPoint>("cloud_projected.pcd",cloud_projected, false);
}
}
2.3 面特征提取
PCL中Sample——consensus模块提供了RANSAC平面拟合模块。
SACMODEL_PLANE 模型：定义为平面模型，共设置四个参数 [normal_x，normal_y，normal_z，d]。其中，（normal_x，normal_y，normal_z）为平面法向量 $( A , B , C )$，$d$ 为常数项$D$。
//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 可选择配置，设置模型系数需要优化
seg.setOptimizeCoefficients(true);
// 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云
seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型
seg.setMethodType(pcl::SAC_RANSAC); //设置随机采样一致性方法类型
seg.setDistanceThreshold(0.01); //设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
//表示点到估计模型的距离最大值，
seg.setInputCloud(cloud);
//引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients
seg.segment(*inliers, *coefficients);
2.4 圆柱体提取
圆柱体的提取也是基于Ransec来实现提取，RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点（inliers），简称内点，否则为模型外样本点（outliers），简称外点。
//-----------------------------法线估计--------------------------------
cout << "->正在计算法线..." << endl;
pcl::NormalEstimation<PointT, pcl::Normal> ne; // 创建法向量估计对象
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
ne.setSearchMethod(tree); // 设置搜索方式
ne.setInputCloud(cloud); // 设置输入点云
ne.setKSearch(50); // 设置K近邻搜索点的个数
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
ne.compute(*cloud_normals); // 计算法向量，并将结果保存到cloud_normals中
//=====================================================================
//----------------------------圆柱体分割--------------------------------
cout << "->正在圆柱体分割..." << endl;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; // 创建圆柱体分割对象
seg.setInputCloud(cloud); // 设置输入点云：待分割点云
seg.setOptimizeCoefficients(true); // 设置对估计的模型系数需要进行优化
seg.setModelType(pcl::SACMODEL_CYLINDER); // 设置分割模型为圆柱体模型
seg.setMethodType(pcl::SAC_RANSAC); // 设置采用RANSAC算法进行参数估计
seg.setNormalDistanceWeight(0.1); // 设置表面法线权重系数
seg.setMaxIterations(10000); // 设置迭代的最大次数
seg.setDistanceThreshold(0.05); // 设置内点到模型距离的最大值
seg.setRadiusLimits(3.0, 4.0); // 设置圆柱模型半径的范围
seg.setInputNormals(cloud_normals); // 设置输入法向量
pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices); // 保存分割结果
pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients); // 保存圆柱体模型系数
seg.segment(*inliers_cylinder, *coefficients_cylinder); // 执行分割，将分割结果的索引保存到inliers_cylinder中，同时存储模型系数coefficients_cylinder
cout << "\n\t\t-----圆柱体系数-----" << endl;
cout << "轴线一点坐标：(" << coefficients_cylinder->values[0] << ", "
<< coefficients_cylinder->values[1] << ", "
<< coefficients_cylinder->values[2] << ")"
<< endl;
cout << "轴线方向向量：(" << coefficients_cylinder->values[3] << ", "
<< coefficients_cylinder->values[4] << ", "
<< coefficients_cylinder->values[5] << ")"
<< endl;
cout << "圆柱体半径：" << coefficients_cylinder->values[6] << endl;
2.5 半径近邻
半径内近邻搜索（Neighbors within Radius Search），是指搜索点云中一点在球体半径 R内的所有近邻点。
//---------------------------------- 半径R近邻搜索 ------------------------------
pcl::KdTreeFLANN<pcl::PointXYZ>kdtree; //创建kdtree对象
kdtree.setInputCloud(cloud); //设置搜索空间
pcl::PointXYZ searchPoint; //定义查询点
searchPoint = cloud->points[0];
cout << "->查询点坐标为：" << searchPoint << endl;
float R = 0.1; //设置搜索半径大小
vector<int> pointIdxRadiusSearch; //存储近邻索引
vector<float> pointRadiusSquaredDistance; //存储近邻对应的平方距离
cout << "\n->正在进行半径R邻域近邻搜索..." << endl << endl;
if (kdtree.radiusSearch(searchPoint, R, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
//打印所有近邻点坐标，方式2
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
{
cout << "第" << i + 1 << "个近邻点："
<< cloud->points[pointIdxRadiusSearch[i]]
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;
}
}
else
{
PCL_ERROR("未搜索到邻近点！\a\n");
}
2.6 聚类
首先选取种子点，利用kd-tree对种子点进行半径r邻域搜索，若邻域内存在点，则与种子点归为同一聚类簇Q；
/--------------------------------- 欧式聚类 ---------------------------------
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); // 创建kd树
tree->setInputCloud(cloud);
vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(0.2); //设置近邻搜索的半径
ec.setMinClusterSize(200); //设置最小聚类点数
ec.setMaxClusterSize(999999); //设置最大聚类点数
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
/// 执行欧式聚类分割，并保存分割结果
int j = 0;
for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
PointCloudT::Ptr cloud_cluster(new PointCloudT);
for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
{
cloud_cluster->points.push_back(cloud->points[*pit]);
}
/*cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;*/
stringstream ss;
ss << "tree_" << j + 1 << ".pcd";
pcl::io::savePCDFileBinary(ss.str(), *cloud_cluster);
cout << "->" << ss.str() << "详情：" << endl;
cout << *cloud_cluster << endl;
j++;
}
2.6 区域生长
区域生长的基本思想是将具有相似性质的点集合起来构成区域。首先对每个需要分割的区域找出一个种子作为生长的起点,然后将种子周围邻域中与种子有相同或相似性质的点(根据事先确定的生长或相似准则来确定，多为法向量、曲率)归并到种子所在的区域中。
//------------------------------- 法线估计 -------------------------------
cout << "->正在估计点云法线..." << endl;
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; //创建法线估计对象ne
pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); //设置搜索方法
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>); //存放法线
ne.setSearchMethod(tree);
ne.setInputCloud(cloud);
ne.setKSearch(20);
ne.compute(*normals);
//========================================================================
//------------------------------- 区域生长 -------------------------------
cout << "->正在进行区域生长..." << endl;
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg; //创建区域生长分割对象
rg.setMinClusterSize(50); //设置满足聚类的最小点数
rg.setMaxClusterSize(99999999); //设置满足聚类的最大点数
rg.setSearchMethod(tree); //设置搜索方法
rg.setNumberOfNeighbours(30); //设置邻域搜索的点数
rg.setInputCloud(cloud); //设置输入点云
rg.setInputNormals(normals); //设置输入点云的法向量
rg.setSmoothnessThreshold(3.0 / 180.0 * M_PI); //设置平滑阈值，弧度，用于提取同一区域的点
rg.setCurvatureThreshold(1.0); //设置曲率阈值，如果两个点的法线偏差很小，则测试其曲率之间的差异。如果该值小于曲率阈值，则该算法将使用新添加的点继续簇的增长
vector<pcl::PointIndices> clusters; //聚类索引向量
rg.extract(clusters); //获取聚类结果，并保存到索引向量中
cout << "->聚类个数为" << clusters.size() << endl;
2.7 线特征拟合
一般线特征拟合的方式前提是先要滤除不必要的点，而这个就需要使用K-D tree来先实现搜索
//----------------------------- 空间直线拟合 -----------------------------
pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud)); //指定拟合点云与几何模型
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_line); //创建随机采样一致性对象
ransac.setDistanceThreshold(0.01); //内点到模型的最大距离
ransac.setMaxIterations(1000); //最大迭代次数
ransac.computeModel(); //执行RANSAC空间直线拟合
vector<int> inliers; //存储内点索引的向量
ransac.getInliers(inliers); //提取内点对应的索引
/// 根据索引提取内点
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_line);
/// 模型参数
Eigen::VectorXf coefficient;
ransac.getModelCoefficients(coefficient);
cout << "直线点向式方程为：\n"
<< " (x - " << coefficient[0] << ") / " << coefficient[3]
<< " = (y - " << coefficient[1] << ") / " << coefficient[4]
<< " = (z - " << coefficient[2] << ") / " << coefficient[5];
2.8 点特征提取
点特征的提取和线特征的提取原理一样
pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI, pcl::Normal> harris;
harris.setInputCloud(cloud);//设置输入点云 指针
harris.setNonMaxSupression(true);
harris.setRadius(0.6f);// 块体半径
harris.setThreshold(0.01f);//数量阈值
//新建的点云必须初始化，清零，否则指针会越界
//注意Harris的输出点云必须是有强度(I)信息的 pcl::PointXYZI，因为评估值保存在I分量里
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_out_ptr(new pcl::PointCloud<pcl::PointXYZI>);
// 计算特征点
harris.compute(*cloud_out_ptr);
3. 参考链接
https://blog.csdn.net/weixin_46098577/article/details/119973826
https://blog.csdn.net/qq_37394634/article/details/119819023
https://github.com/OctoMap/octomap_mapping/blob/kinetic-devel/octomap_server/src/OctomapServer.cpp
https://blog.csdn.net/weixin_42291376/article/details/106154329
https://blog.csdn.net/zzh_AI/article/details/96483769
评论（0）
您还未登录，请登录后发表或查看评论