PCL (Point Cloud Library) 是用于处理2D/3D 图像以及点云的一个大型开源项目。学习PCL最好的途径是阅读其官网文档（Point Cloud Library (PCL)）。虽然PCL的网站文档稍微有点“丑”，但是其内容十分详尽。从应用的角度而言，PCL可以用于点云的分割、分类、校准以及可视化等方面。从理论角度而言，PCL中包含的众多算法能更好得帮助人们理解与创造新的点云算法。无论是工业应用还是科研攻关，PCL都能在三维数据处理领域祝您一臂之力。
激光雷达作为自动驾驶最常用的传感器，经常需要使用激光雷达来做建图、定位和感知等任务。
而这时候使用降低点云规模的预处理方法，可以能够去除无关区域的点以及降低点云规模。并能够给后续的PCL点云分割带来有效的收益。
1. 三维激光雷达压缩成二维
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
void filterGroundPlane(const PCLPointCloud& pc,
PCLPointCloud& ground,
PCLPointCloud& nonground)
{
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 and !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);
CLPointCloud 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. 面特征提取
PCL中Sample——consensus模块提供了RANSAC平面拟合模块。
SACMODEL_PLANE 模型：定义为平面模型，共设置四个参数 [normal_x, normal_y, normal_z, d]。其中，(normal_x, normal_y, normal_z)为平面法向量，d为常数项。
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation& lt;
pcl::PointXYZ& gt;
// 可选择配置，设置模型系数需要优化
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);3. 圆柱体提取
圆柱体的提取也是基于Ransec来实现提取，RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差。
再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点（inliers），简称内点，否则为模型外样本点（outliers），简称外点。
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
// Create the segmentation object for cylinder segmentation and set all the parameters
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_CYLINDER); // 提取圆柱体的操作
seg.setMethodType(pcl::SAC_RANSAC);
seg.setNormalDistanceWeight(0.1);
seg.setMaxIterations(10000);
seg.setDistanceThreshold(0.05); // 距离5cm
seg.setRadiusLimits(0, 0.1); // 半径 10cm
seg.setInputCloud(cloud_filtered2);
seg.setInputNormals(cloud_normals2);
// Obtain the cylinder inliers and coefficients
seg.segment(*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;4. 半径近邻
半径内近邻搜索（Neighbors within Radius Search），是指搜索点云中一点在球体半径 R内的所有近邻点。
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}5. 聚类
首先选取种子点，利用kd-tree对种子点进行半径r邻域搜索，若邻域内存在点，则与种子点归为同一聚类簇Q；
欧式聚类：
void Cvisualization::ShowCloud4()
{
//读入点云数据table_scene_lms400.pcd
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("E:/ai/pcltest/20210903changhuAM-0001.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// /*从输入的.PCD文件载入数据后，我们创建了一个VoxelGrid滤波器对数据进行下采样，我们在这里进行下采样的原 因是来加速处理过程，越少的点意味着分割循环中处理起来越快。*/
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg; //体素栅格下采样对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f); //设置采样的体素大小
vg.filter (*cloud_filtered); //执行采样保存数据
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;//创建分割对象
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true); //设置对估计的模型参数进行优化处理
seg.setModelType (pcl::SACMODEL_PLANE);//设置分割模型类别
seg.setMethodType (pcl::SAC_RANSAC);//设置用哪个随机参数估计方法
seg.setMaxIterations (100); //设置最大迭代次数
seg.setDistanceThreshold (0.02); //设置判断是否为模型内点的距离阈值
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
// /*为了处理点云中包含多个模型，我们在一个循环中执行该过程，并在每次模型被提取后，我们保存剩余的点，进行迭代。模型内点通过分割过程获取，如下*/
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
//移去平面局内点，提取剩余点云
pcl::ExtractIndices<pcl::PointXYZ> extract; //创建点云提取对象
extract.setInputCloud (cloud_filtered); //设置输入点云
extract.setIndices (inliers); //设置分割后的内点为需要提取的点集
extract.setNegative (false); //设置提取内点而非外点
// Get the points associated with the planar surface
extract.filter (*cloud_plane); //提取输出存储到cloud_plane
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered); //创建点云索引向量，用于存储实际的点云信息
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.2); //设置近邻搜索的搜索半径为2cm
ec.setMinClusterSize (100);//设置一个聚类需要的最少点数目为100
ec.setMaxClusterSize (25000);//设置一个聚类需要的最大点数目为25000
ec.setSearchMethod (tree);//设置点云的搜索机制
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);//从点云中提取聚类，并将点云索引保存在cluster_indices中
// /* 为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中 */
//迭代访问点云索引cluster_indices，直到分割出所有聚类
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
//创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "E:/ai/pcltest/cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("HelloMyFirstVisualPCL"));
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}6. 区域生长
区域生长的基本思想是将具有相似性质的点集合起来构成区域。
首先对每个需要分割的区域找出一个种子作为生长的起点,然后将种子周围邻域中与种子有相同或相似性质的点（根据事先确定的生长或相似准则来确定，多为法向量、曲率）归并到种子所在的区域中。
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/visualization/cloud_viewer.h>
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile("data//table_scene_lms400.pcd", *cloud) == -1)
{
std::cout << "Cloud reading failed." << std::endl;
return (-1);
}
// 设置搜索方式为kdTree
pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
// 计算法向量
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(50);
normal_estimator.compute(*normals);
//直通滤波在Z轴的0到1米之间
pcl::IndicesPtr indices(new std::vector <int>);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 1.0);
pass.filter(*indices);
// 欧式聚类
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize(5000); //最小的聚类的点数
reg.setMaxClusterSize(1000000); //最大的聚类的点数
reg.setSearchMethod(tree); //搜索方式
reg.setNumberOfNeighbours(30); //设置搜索的邻域点的个数
reg.setInputCloud(cloud); //输入点
//reg.setIndices (indices);
reg.setInputNormals(normals); //输入的法线
reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI); //设置平滑度
reg.setCurvatureThreshold(1.0); //设置曲率的阀值
// 获取聚类的结果，分割结果保存在点云索引的向量中
std::vector <pcl::PointIndices> clusters;
reg.extract(clusters);
//输出聚类的数量
std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
// 输出第一个聚类的数量
std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
while (counter < clusters[0].indices.size())
{
std::cout << clusters[0].indices[counter] << ", ";
counter++;
if (counter % 10 == 0)
std::cout << std::endl;
}
std::cout << std::endl;
//可视化聚类的结果
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
pcl::visualization::CloudViewer viewer("Cluster viewer");
viewer.showCloud(colored_cloud);
while (!viewer.wasStopped())
{
}
return (0);
}
7. 线特征拟合
一般线特征拟合的方式前提是先要滤除不必要的点，而这个就需要使用K-D tree来先实现搜索
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
using namespace std::chrono_literals;
pcl::visualization::PCLVisualizer::Ptr
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
// viewer->addCoordinateSystem (1.0, "global");
//viewer->initCameraParameters();
return (viewer);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr
create_line(double x0, double y0, double z0, double a, double b, double c, double point_size = 1000, double step = 0.1)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
cloud_line->width = point_size;
cloud_line->height = 1;
cloud_line->resize(cloud_line->width * cloud_line->height);
for (std::size_t i = 0; i < cloud_line->points.size(); ++i) {
cloud_line->points[i].x = x0 + a / std::pow(a * a + b * b + c * c, 0.5) * i * 0.1;
cloud_line->points[i].y = y0 + b / std::pow(a * a + b * b + c * c, 0.5) * i * 0.1;
cloud_line->points[i].z = z0 + c / std::pow(a * a + b * b + c * c, 0.5) * i * 0.1;
}
return cloud_line;
}
void fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double distance_threshold)
{
// fit line from a point cloud
pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers1(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_LINE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(distance_threshold);
seg.setInputCloud(cloud);
seg.segment(*inliers1, *coefficients1);
// line parameters
double x0, y0, z0, a, b, c;
x0 = coefficients1->values[0];
y0 = coefficients1->values[1];
z0 = coefficients1->values[2];
a = coefficients1->values[3];
b = coefficients1->values[4];
c = coefficients1->values[5];
std::cout << "model parameters1:"
<< " (x - " << x0 << ") / " << a << " = (y - " << y0 << ") / " << b
<< " = (z - " << z0 << ") / " << c << std::endl;
// extract segmentation part
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers1);
extract.setNegative(false);
extract.filter(*cloud_line1);
// extract remain pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_remain(new pcl::PointCloud<pcl::PointXYZ>);
extract.setNegative(true);
extract.filter(*cloud_remain);
//显示原始点云
pcl::visualization::PCLVisualizer::Ptr viewer_ori;
viewer_ori = simpleVis(cloud);
while (!viewer_ori->wasStopped()) {
viewer_ori->spinOnce(100);
std::this_thread::sleep_for(100ms);
}
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud_remain, "cloud_remain");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_remain");
viewer->addPointCloud<pcl::PointXYZ>(cloud_line1, "cloud_line1");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud_line1");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.5, 0.5, "cloud_line1");
while (!viewer->wasStopped()) {
viewer->spinOnce(100);
std::this_thread::sleep_for(100ms);
}
}
void demo()
{
// line parameters
double x0 = -2, y0 = -2, z0 = 0, a = 1, b = 1, c = 0;
auto line_pcd_create = create_line(x0, y0, z0, a, b, c);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_noise(new pcl::PointCloud<pcl::PointXYZ>);
std::size_t noise_points_size = line_pcd_create->points.size() / 10;
cloud_noise->width = noise_points_size;
cloud_noise->height = 1;
cloud_noise->points.resize(cloud_noise->width * cloud_noise->height);
// add noise
for (std::size_t i = 0; i < noise_points_size; ++i) {
int random_num = line_pcd_create->points.size() * rand() / (RAND_MAX + 1.0f);
cloud_noise->points[i].x =
line_pcd_create->points[random_num].x + 10 * rand() / (RAND_MAX + 1.0f) - 5;
cloud_noise->points[i].y =
line_pcd_create->points[random_num].y + 10 * rand() / (RAND_MAX + 1.0f) - 5;
cloud_noise->points[i].z =
line_pcd_create->points[random_num].z + 10 * rand() / (RAND_MAX + 1.0f) - 5;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr line_with_noise(new pcl::PointCloud<pcl::PointXYZ>);
*line_with_noise = *cloud_noise + *line_pcd_create;
fit_line(line_with_noise, 1);
}
int main(int argc, char* argv[])
{
if (argc < 3) {
std::cout << "please input parametars:\nfilepath\ndistance_threshold" << std::endl;
demo();
return -1;
}
std::string file_path = argv[1];
double distance_threshold = atof(argv[2]);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile(file_path, *cloud) < 0) {
std::cout << "can not read file " << file_path << std::endl;
return -1;
}
std::cout << "point size: " << cloud->points.size() << std::endl;
fit_line(cloud, distance_threshold);
return 0;
}
8. 点特征提取
点特征的提取和线特征的提取原理一样
pcl::HarrisKeypoints3D<pcl::PointXYZ, pcl::PointXYZI> harris;
harris.setInputCloud(cloud); //设置输入点云 指针
harris.setNonMaxSupression(true);
harris.setRadius(0.6f); // 块体半径
harris.setThreshold(0.01f); //数量阈值
//新建的点云必须初始化，清零，否则指针会越界
//注意Harris的输出点云必须是有强度(I)信息的 pcl::PointXYZI，因为评估值保存在I分量里
pcl::PointCloud::Ptr cloud_out_ptr(new pcl::PointCloud);
// 计算特征点
harris.compute(*cloud_out_ptr);
扫一扫
100
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
