PCL手册
一、读取点云
//-------------------导入模板点云------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src_o(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("F:\\workwork20211012\\PointCloudTypeRecog\\workfile\\surface_raw.pcd", *cloud_src_o) == -1) //* load the file
{
return (-1);
}else {
//成功导入
qDebug()<<"Source clouds load ok!";
// for(int i=0;i<cloud_src_o->size();i++)
// {
// qDebug()<<"cloud_src_o点坐标x"<<cloud_src_o->points[i].PointXYZ::x;
// qDebug()<<"cloud_src_o点坐标y"<<cloud_src_o->points[i].PointXYZ::y;
// qDebug()<<"cloud_src_o点坐标z"<<cloud_src_o->points[i].PointXYZ::z;
// }
}
二、过滤NAN值点
//过滤掉NAN点
std::vector<int> tgt_mapping_out;
pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_temp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::removeNaNFromPointCloud(*cloud_tgt_o, *tgt_temp, tgt_mapping_out);//参数1：原始点云；参数2：去除NAN值点的点云；参数3：点云索引
三、获取点云在X、Y、Z三个轴的最大值和最小值
pcl::PointXYZ XYZmin;//用于存放三个轴的最小值
pcl::PointXYZ XYZmax;//用于存放三个轴的最大值
pcl::getMinMax3D(*tgt_temp,XYZmin,XYZmax);
四、直通滤波（按照坐标轴范围过滤点云）
double Tgt_threshold=(XYZmin.z+PointLoc*delta);//0.5415;
//-------------------直通滤波,按照Z轴过滤场景点云------------------------
/* 声明 直通滤波 后 的点云 */
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_PassThrough_filtered (new pcl::PointCloud<pcl::PointXYZ>);
/* 声明直通滤波 的 类实例 */
pcl::PassThrough<pcl::PointXYZ> pass;
/* 设置输入点云 */
pass.setInputCloud(cloud_tgt_o);
/* 设置滤波的维度 */
pass.setFilterFieldName ("z");
/* 设置滤波的范围 */
pass.setFilterLimits (0,Tgt_threshold);
/* 设置去掉的 是 范围内（true） 还是 范围外（false） */
pass.setFilterLimitsNegative(0);//将这个改为0 即 去除范围外的点 的效果
/* 执行滤波 返回 滤波后 的 点云 */
pass.filter(*cloud_PassThrough_filtered);
五、点云可视化
//-------------------原始点云和过滤目标点云可视化------------------------
pcl::visualization::PCLVisualizer viewer00("Visualization of original and target point cloud");
viewer00.setBackgroundColor(255,255,255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_src_o, 0, 255, 0);
viewer00.addPointCloud<pcl::PointXYZ>(cloud_src_o, single_color, "Source clouds");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_PassThrough_filtered, 255, 0, 0);
viewer00.addPointCloud<pcl::PointXYZ>(cloud_PassThrough_filtered, single_color2, "Target clouds");
while (!viewer00.wasStopped())
{
viewer00.spinOnce(1);
}
六、点云坐标变换
//-------------------定义变化矩阵------------------------
float theta = M_PI/2; //旋转的角度
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();//定义坐标变换矩阵
// 定义在x轴上的平移，2.5m
//transform_2.translation() << 2.5, 0.0, 0.0; // 三个数分别对应X轴、Y轴、Z轴方向上的平移
// 定义旋转矩阵，绕z轴
transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitX())); //同理，UnitX(),绕X轴；UnitY(),绕Y轴.
//定义旋转后点云
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::transformPointCloud (*cloud_src_o, *transformed_cloud, transform_2);//点云按照transform_2进行坐标变换
七、点云保存为图片
//保存点云为图片
float Zoom=0.001;
std::vector<cv::Point2i> points;
for(int i=0; i<cloud_src_o->points.size(); i++)
{
cv::Point2i point;
point.x = cloud_src_o->points[i].x/Zoom;
point.y = cloud_src_o->points[i].y/Zoom;
points.push_back(point);
}
cv::Rect box = cv::boundingRect(cv::Mat(points));
cout << "cv::Rect box br x y " << box.br().x << " " << box.br().y << endl;
cout << "cv::Rect box tl x y " << box.tl().x << " " << box.tl().y << endl;
cout << "cv::Area box " << box.area() << endl;
//create Mat image
int rows = box.height;
int cols = box.width;
cv::Mat map_image(cols,rows, CV_8UC1, 255);
for(int i=0; i<cloud_src_o->points.size(); i++)
{
int x = (cloud_src_o->points[i].x/Zoom - box.tl().x);
int y = (cloud_src_o->points[i].y/Zoom - box.tl().y);
if( x > 0 && x < cols
&& y > 0 && y < rows)
{
int value_change = map_image.at<uchar>(x,y) * 0.9;
map_image.at<uchar>(x,y) = value_change;
}
}
cv::imwrite("global_map.png", map_image);
八、转换为深度图
-------------------三维点云转换为深度图：设置转换参数------------------------
//设置角度分辨率 弧度 相邻像素点 所对应的每束 光束之间相差 1 度
float angularResolution = (float)( 1.0f*(M_PI/180.0f) );
//模拟的距离传感器对周围环境 有360度的视场角
float maxAngleWidth = (float)( 360.0f*(M_PI/180.0f) );
//相当于前面所有的 视角
float maxAngleHeight = (float)( 180.0f*(M_PI/180.0f) );
//传感器位置
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
//传感器的坐标系 各轴方向
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
//传感器噪声
float noiseLevel=0.00;
//最小距离
float minRange = 0.0f;
//在剪裁图像时若果 此值>0，将在图像周围留下当前视点不可见的边界
int borderSize = 1;
-------------------模板深度图的保存------------------------
pcl::RangeImage SrcRangeImage;//声明深度图像
/* 将点云转为 深度图像的 操作 */
SrcRangeImage.createFromPointCloud(*cloud_src_o, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
//保存模板点云的深度图
float* SrcRanges = SrcRangeImage.getRangesArray();
unsigned char* SrcRGBImage = pcl::visualization::FloatImageUtils::getVisualImage(SrcRanges, SrcRangeImage.width, SrcRangeImage.height);
pcl::io::saveRgbPNGFile("SrcRangeImage.png", SrcRGBImage, SrcRangeImage.width, SrcRangeImage.height);
//-------------------场景深度图的保存------------------------
pcl::RangeImage TgtRangeImage;//声明深度图像
/* 将点云转为 深度图像的 操作 */
TgtRangeImage.createFromPointCloud(*cloud_PassThrough_filtered, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
//保存场景点云的深度图
float* TgtRanges = TgtRangeImage.getRangesArray();
unsigned char* TgtRGBImage = pcl::visualization::FloatImageUtils::getVisualImage(TgtRanges, TgtRangeImage.width, TgtRangeImage.height);
pcl::io::saveRgbPNGFile("TgtRangeImage.png", TgtRGBImage, TgtRangeImage.width, TgtRangeImage.height);
九、保存pcd格式点云
std::string str = "/home/tagelyx/data/map/test/build/" + std::to_string(j)+".pcd";
pcl::io::savePCDFileASCII (str, *cloud_cluster);
十、3D-NDT配准算法
//3D-NDT配准算法
//将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度。
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize(0.02, 0.02, 0.02);
approximate_voxel_filter.setInputCloud(Src_Cloud_Planar);
approximate_voxel_filter.filter(*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size()
<< " data points from room_scan2.pcd" << std::endl;
//初始化正态分布变换（NDT）
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
//设置依赖尺度NDT参数
//为终止条件设置最小转换差异
ndt.setTransformationEpsilon(0.01);
//为More-Thuente线搜索设置最大步长
ndt.setStepSize(0.1);
//设置NDT网格结构的分辨率（VoxelGridCovariance）
ndt.setResolution(1.0);
//设置匹配迭代的最大次数
ndt.setMaximumIterations(35);
// 设置要配准的点云
ndt.setInputCloud(filtered_cloud);
//设置点云配准目标
ndt.setInputTarget(Tgt_Cloud_Planar);
//设置使用机器人测距法得到的初始对准估计结果
Eigen::AngleAxisf init_rotation(0, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(0, 0, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
//计算需要的刚体变换以便将输入的点云匹配到目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
<< " score: " << ndt.getFitnessScore() << std::endl;
//使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud(*Src_Cloud_Planar, *output_cloud, ndt.getFinalTransformation());
//保存转换的输入点云
//pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);
// 初始化点云可视化界面
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer_final->setBackgroundColor(0, 0, 0);
//对目标点云着色（红色）并可视化
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color(Tgt_Cloud_Planar, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ>(Tgt_Cloud_Planar, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
//对转换后的目标点云着色（绿色）并可视化
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color(output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// 启动可视化
viewer_final->addCoordinateSystem(1.0);
viewer_final->initCameraParameters();
//等待直到可视化窗口关闭。
while (!viewer_final->wasStopped())
{
viewer_final->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
十一、四点法配准4pcs
//四点法配准4pcs
pcl::PointCloud<pcl::PointXYZ>::Ptr pcs(new pcl::PointCloud<pcl::PointXYZ>);
pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
fpcs.setInputSource(Src_Cloud_Planar);//输入待配准点云
fpcs.setInputTarget(Tgt_Cloud_Planar);//输入目标点云
//参数设置
fpcs.setApproxOverlap(0.7);//两点云重叠度
fpcs.setDelta(0.05);//Bunny
//fpcs.setDelta(0.5);//hippo
fpcs.setMaxComputationTime(50);
fpcs.setNumberOfSamples(1000);
Eigen::Matrix4f tras;
clock_t start = clock();
fpcs.align(*pcs);
clock_t end = clock();
cout << "time:" << (double)(end - start) / (double)CLOCKS_PER_SEC << endl;
cout << "score:" << fpcs.getFitnessScore() << endl;
tras = fpcs.getFinalTransformation();
cout << "matrix:" << endl << tras << endl << endl << endl;
qDebug()<<"002!";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_end(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*Src_Cloud_Planar, *cloud_end, tras);
visualize_pcd(Src_Cloud_Planar, Tgt_Cloud_Planar, cloud_end);
qDebug()<<"003!";
十二、Ransac采样一致性配准
//配准方法1*********************************************
//【4】设置搜索方法
SearchT::Ptr tree(new SearchT);
//【5】计算对象和场景点云的FPFH特征
pcl::console::print_highlight ("Estimating features...\n");
FeatureCloudT::Ptr object_f=computeFeature(Src_Cloud_Planar,tree);
FeatureCloudT::Ptr scene_f=computeFeature(Tgt_Cloud_Planar,tree);
//【6】实时配准
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointT,PointT,FeatureT> align;//设置配准对象
align.setInputSource(Src_Cloud_Planar);//添加原始对象点云
align.setSourceFeatures(object_f); //添加对象特征点云
align.setInputTarget(Tgt_Cloud_Planar);//添加原始场景点云
align.setTargetFeatures(scene_f);//添加场景特征点云
//设置配准参数
align.setMaximumIterations (50000); // 采样一致性迭代次数
align.setNumberOfSamples (3); // 创建假设所需的样本数
align.setCorrespondenceRandomness (5); // 使用的临近特征点的数目
align.setSimilarityThreshold (0.95f); // 多边形边长度相似度阈值
align.setMaxCorrespondenceDistance (2.5f * 0.005); // 判断是否为内点的距离阈值
align.setInlierFraction (0.75f); //接受位姿假设所需的内点比例
//return align;
PointCloudT::Ptr object_aligned(new PointCloudT);
align.align(*object_aligned); //计算得到对象配准后的点云
//【7】计算变换矩阵
if(!align.hasConverged())
{
pcl::console::print_error ("Alignment failed!\n");
return (-1);
}
else
{
Eigen::Matrix4f transformation = align.getFinalTransformation();
//输出变换矩阵
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), cloud_src_o->size ());
}
//【8】可视化
pcl::visualization::PCLVisualizer Viwer("3D Viwer");
int v1(0),v2(0);
Viwer.createViewPort(0,0,0.5,1,v1);
Viwer.createViewPort(0.5,0,1,1,v2);
Viwer.setBackgroundColor(255,255,255,v1);
Viwer.addPointCloud (Tgt_Cloud_Planar, ColorHandlerT (Tgt_Cloud_Planar, 0.0, 255.0, 0.0), "scene",v1);
Viwer.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned",v1);
Viwer.addPointCloud(Src_Cloud_Planar,ColorHandlerT (Src_Cloud_Planar, 0.0, 255.0, 0.0), "object_before_aligned",v2);
Viwer.addPointCloud(Tgt_Cloud_Planar,ColorHandlerT (Tgt_Cloud_Planar, 0.0, 0.0, 255.0), "scene_v2",v2);
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene");
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_aligned");
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_before_aligned");
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene_v2");
Viwer.spin ();
十三、计算FPFH特征
FeatureCloudT::Ptr computeFeature(PointCloudT::Ptr input_cloud,SearchT::Ptr tree)
{
//【1】在计算FPFH特征点云之前需要首先计算法向量点云
//定义原始点云对应的法向量点云
NormalCloudT::Ptr point_normal(new NormalCloudT);
//定义法向量估计对象
pcl::NormalEstimation<PointT,NormalT>en;
//设置en估计参数
en.setInputCloud(input_cloud); //设置带估计的点云
en.setSearchMethod(tree); //设置搜索的方法
en.setKSearch(10); //设置领域的大小K为10；
en.compute(*point_normal);//设置计算结果的保存对象
//【2】在得到原始点云对应的法向量点云后，计算FPFH特征点云
FeatureCloudT::Ptr featureCloud(new FeatureCloudT);
//pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_fpfh;
FeatureEstimationT fe;
fe.setInputCloud(input_cloud); //设置原始点云
fe.setInputNormals(point_normal);//设置计算得到的法向量点云
fe.setSearchMethod(tree);//设置搜索方法
fe.setKSearch(10); //设置搜索邻域大小为10
fe.compute(*featureCloud);
return featureCloud;
}
十四、画圆形
//画圆环
QVector<pcl::PointXYZ> points;
double o_x = modelParas(0), o_y = modelParas(1), r = modelParas(2); //圆心及半径
for (int i = 0; i < 100; i++) //计算圆环上点的坐标
{
pcl::PointXYZ point;
double alpha = 2 * M_PI / (100 - 1);
point.x = o_x + r * cos(i * alpha);
point.y = o_y + r * sin(i * alpha);
point.z = 0;
points.push_back(point);
}
//将圆环上的点用线段连起来
for (int i = 0; i < points.size() - 1; i++)
{
viewer00.addLine(points.at(i), points.at(i + 1), QString("CircleLineX%1").arg(i).toStdString(), 0);
}
viewer00.addLine(points.at(points.size() - 1), points.at(0), QString("CircleLineX%1_%2").arg(points.size()).arg(5).toStdString(), 0);
十五、显示窗口操作
版权声明：本文为kissgoodbye2012原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。