PCL基本操作大全_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;
        //        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

二、过滤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：点云索引

1
2
3
4
5

三、获取点云在X、Y、Z三个轴的最大值和最小值

	pcl::PointXYZ XYZmin;//用于存放三个轴的最小值
    pcl::PointXYZ XYZmax;//用于存放三个轴的最大值
    pcl::getMinMax3D(*tgt_temp,XYZmin,XYZmax);
1
2
3

四、直通滤波（按照坐标轴范围过滤点云）

	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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

五、点云可视化

//-------------------原始点云和过滤目标点云可视化------------------------
    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);
    }
1
2
3
4
5
6
7
8
9
10
11

六、点云坐标变换

//-------------------定义变化矩阵------------------------
    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进行坐标变换
1
2
3
4
5
6
7
8
9
10
11

七、点云保存为图片

//保存点云为图片
    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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

八、转换为深度图

-------------------三维点云转换为深度图：设置转换参数------------------------
            //设置角度分辨率  弧度 相邻像素点 所对应的每束 光束之间相差 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);
    
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

九、保存pcd格式点云

std::string str = "/home/tagelyx/data/map/test/build/" + std::to_string(j)+".pcd";
pcl::io::savePCDFileASCII (str, *cloud_cluster);
1
2

十、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));
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

十一、四点法配准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!";
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

十二、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 ();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63

十三、计算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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

十四、画圆形

//画圆环
        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);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

十五、显示窗口操作

可视化快捷键