PCL系列文章——第二篇，PCL的使用

悠悠 2023-06-24 11:22 5阅读 0赞

> PCL是点云处理一个非常强大的库，可以完成多种点云的操作算法；而VTK又是三维点云显示以及三维重建非常有用的库。虽然只会调库难免让人绝对低端，但这些库的功能也是真的强大。

PCL系列文章

[第一篇，PCL+VTK在windows下的安装][PCL_VTK_windows]

[第二篇，PCL的使用][PCL]

[如果需要PCL+VTK的安装文件，咸鱼链接：][PCL_VTK]

[如果需要三维点云配准的qt程序，咸鱼链接][qt]

--------------------

**目录**

第一、导入ply文件

第二、点云可视化

第三、将三维点云转换为BA图（BearingAngleImage）

第四、使用opencv中的gms算法进行错误匹配点筛选

第五、将二维图像匹配点重映射为三维点云对

第六、去除NAN点

第七、用索引复制一个新的点云

第八、保存点云为ply格式

第九、对应三维点云对连线的可视化

第十、根据匹配的三维点云对求刚体变换矩阵

第十一、使用刚体变换矩阵对点云进行旋转平移操作

第十二、icp进行精确匹配

--------------------

# 第一、导入ply文件 #

pcl支持很多种数据格式，这里介绍ply格式。这个ply格式有很多参数，不同ply文件包含的内容也可能不同，如有些只有点，有些有点有面，有些有点有颜色。**所以导入会造成很多nan点，这一点一定要在后面的点云处理中注意。**

#include <pcl/io/pcd_io.h>
    #include <pcl/io/ply_io.h>
    #include <pcl/io/png_io.h>
     
       //导入ply格式的点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src_o(new pcl::PointCloud<pcl::PointXYZ>);
        if (pcl::io::loadPLYFile<pcl::PointXYZ>("./dragonStandRight_24.ply", *cloud_src_o) == -1) //* load the file
        {
            return (-1);
        }else {
            //成功导入
            qDebug()<<"src loads 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;
            //        }
        }

# 第二、点云可视化 #

导入的点云需要通过vtk库进行可视化，方便调试。

首先，定义PCLVisualizer类的对象；

然后，可以设置界面的背景颜色，点的颜色；

再添加点的数据；

最后进行显示。

//引入vtk进行点云三维显示
    #include <pcl/visualization/pcl_visualizer.h>
    
    //原始点云和目标点云可视化
        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, "sample cloud");
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_tgt_o, 255, 0, 0);
        viewer00.addPointCloud<pcl::PointXYZ>(cloud_tgt_o, single_color2, "sample cloud2");
        while (!viewer00.wasStopped())
        {
            viewer00.spinOnce(1);
        }

# 第三、将三维点云转换为BA图（BearingAngleImage） #

这个很多博客都没有，我也是在国外论坛看到过一点，但是它代码还有问题，这次回报中文博客社区啦。

什么是BA图呢？如下定义。我们把三维点云转换为二维图像，提取特征点，匹配特征点，然后将匹配的特征点重新转换为三维的匹配点，就可以得到匹配的三维点云对。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70][]

#include <pcl/io/png_io.h>
    #include <pcl/range_image/bearing_angle_image.h>
    
    //3d点云转换为BA图。第一步：三维点云转换为二维图像
        pcl::BearingAngleImage cloud_src_2d;
        pcl::BearingAngleImage cloud_tgt_2d;
        cloud_src_2d.generateBAImage(*cloud_src_o);
        cloud_tgt_2d.generateBAImage(*cloud_tgt_o);
    
        //保存BA图
        cv::Mat Im(cloud_src_2d.height, cloud_src_2d.width, CV_8UC1, cv::Scalar(255));
        int count = 0;
    //    qDebug()<<"点云集1的宽度数："<<cloud_src_2d.width;
    //    qDebug()<<"点云集1的高度数："<<cloud_src_2d.height;
        for (int i=0; i< cloud_src_2d.height; i++)
        {
            for (int j=0; j< cloud_src_2d.width; j++)
            {
                Im.at<uchar>(i,j) = (cloud_src_2d.points[count].rgba >> 8) & 0xff;
                count++;
            }
        }
        imwrite("src_BA.jpg", Im);

# 第四、使用opencv中的gms算法进行错误匹配点筛选 #

这里是opencv的使用，需要注意：matchGMS函数只在opencv的高版本有，大概是opencv3.4以上吧，这里我用的是opencv4.1，能用高版本尽量高版本。

//剔除错误匹配
        //GMS进行匹配点筛选
        cv::xfeatures2d::matchGMS(Im.size(),Im2.size(),keypoints_1,keypoints_2, matchesAll, matchesGMS);//原始代码
        std::cout << "正确匹配点数: " << matchesGMS.size() << std::endl;

# 第五、将二维图像匹配点重映射为三维点云对 #

BA图的点(i,j)与有序点云的第i扫描层，第j列是一一对应关系的，因此可以得出匹配的三维点云对。

//根据匹配点序号得到匹配后的三维点云对
        vector<int> src_matchpoint_Index;
        vector<int> tgt_matchpoint_Index;
    
        for(int p=0;p<matchesGMS.size();p++)
        {
            src_matchpoint_Index.push_back(cloud_src_2d.width*true_keypoints_1[p].pt.y+true_keypoints_1[p].pt.x);
    //        qDebug()<<"点坐标1x"<<true_keypoints_1[p].pt.x;
    //        qDebug()<<"点坐标1y"<<true_keypoints_1[p].pt.y;
            tgt_matchpoint_Index.push_back(cloud_tgt_2d.width*true_keypoints_2[p].pt.y+true_keypoints_2[p].pt.x);
    //        qDebug()<<"点坐标2x"<<true_keypoints_2[p].pt.x;
    //        qDebug()<<"点坐标2y"<<true_keypoints_2[p].pt.y;
        }

# 第六、去除NAN点 #

其中vector<int>定义的是非nan点的序号。removeNaNFromPointCloud里，第一个参数是输入点云，第二个参数是去除nan点的点云，第三个参数是非nan点的索引。

去除nan点是按照顺序消除nan的，因此排列关系是线性的，可知的。

//过滤掉NAN点
        std::vector<int> src_mapping_out;
        std::vector<int> tgt_mapping_out;
        pcl::PointCloud<pcl::PointXYZ>::Ptr src_temp(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_temp(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::removeNaNFromPointCloud(*src_cloudOut, *src_temp, src_mapping_out);
        pcl::removeNaNFromPointCloud(*tgt_cloudOut, *tgt_temp, tgt_mapping_out);

# 第七、用索引复制一个新的点云 #

已知一个点云的某些点的索引，想要将这些点单独提出来，组成新点云。可以使用如下代码

//src_cloudout和tgt_cloudout已经是一一对应啦
        pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*cloud_src_o, src_matchpoint_Index, *src_cloudOut);
        pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*cloud_tgt_o, tgt_matchpoint_Index, *tgt_cloudOut);

# 第八、保存点云为ply格式 #

这个比较简单，需要注意的是保存前点云文件里是否有nan点，先去除nan点，再保存。

//保存为ply格式
        std::cerr << "Saving to ply file " << std::endl;
        pcl::io::savePLYFileASCII("./src_cloudOut.ply",*true_src_cloudOut);//我去，少了个* 就不行了。有些博客害人啊

# 第九、对应三维点云对连线的可视化 #

使用vtk进行匹配点云的连线图可视化。

首先定义一个可以容纳多个点云和点云连线的ctkCellArray类对象；

然后，添加需要显示的点，使用循环添加；

然后定义pdata，包含前面的点云和点云连线；

最后进行显示。

//对应点云连线
        vtkSmartPointer<vtkCellArray> lines =vtkSmartPointer<vtkCellArray>::New();//连线集合
        for(int i=0;i<true_tgt_cloudOut->size();i++)
        {
            vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
            //第一个参数是序号，第二个参数是点的序号
            line->GetPointIds()->SetId(0,i);
            line->GetPointIds()->SetId(1,true_tgt_cloudOut->size()+i);
            lines->InsertNextCell(line);
        }
    
        //存储点云和连线的数据集
        vtkSmartPointer<vtkPolyData> pdata =vtkSmartPointer<vtkPolyData>::New();
        //添加所有的点
        pdata->SetPoints(pts);
        //添加所有的线
        pdata->SetLines(lines);
    
        //vtk中绘图
        vtkPolyDataMapper *mapper = vtkPolyDataMapper::New();
        mapper->SetInputData(pdata);
    
        vtkSmartPointer<vtkActor> actor =vtkSmartPointer<vtkActor>::New();
        actor->SetMapper(mapper);
        actor->GetProperty()->SetColor(1, 0, 0);
        actor->GetProperty()->SetLineWidth(4);
    
        vtkSmartPointer<vtkRenderer> renderer =vtkSmartPointer<vtkRenderer>::New();
        vtkSmartPointer<vtkRenderWindow> renderWindow = vtkSmartPointer<vtkRenderWindow>::New();
        renderWindow->AddRenderer(renderer);
        vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =vtkSmartPointer<vtkRenderWindowInteractor>::New();
        renderWindowInteractor->SetRenderWindow(renderWindow);
    
        renderer->AddActor(actor);
        renderer->SetBackground(1, 1, 1);
        renderWindow->Render();
        renderWindowInteractor->Start();

# 第十、根据匹配的三维点云对求刚体变换矩阵 #

注意，这里的点云是一一对应的，所谓**一一对应是指：第一个点云第一个点和第二个点云第一个点对应**。要用好这个函数，需要注意这一点哈！

//根据匹配的三维点对求RT矩阵,true_src_cloudOut,true_tgt_cloudOut
        pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> TESVD;
        pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation_matrix;
        TESVD.estimateRigidTransformation(*true_src_cloudOut,*true_tgt_cloudOut,transformation_matrix);

# 第十一、使用刚体变换矩阵对点云进行旋转平移操作 #

函数的第一个参数：待处理的点云；第二个参数：旋转平移后的点云；第三个参数：刚体变换矩阵。

下面我只定义了这个刚体变换矩阵，没有赋值，少了这一步要注意哦，可以参考其他博客进行刚体变换矩阵的定义。

Eigen::Matrix4f icp_trans;
    
    //使用创建的变换对未过滤的输入点云进行变换
        pcl::transformPointCloud(*origin_src_nonan, *icp_result, icp_trans);

# 第十二、icp进行精确匹配 #

最重要的是：其中的transformation\_matrix是刚体变换矩阵的初值，这个值可以通过粗配准得到。

注意：icp非常依赖这个初值，很多研究也在解决这个问题。

//icp配准，较为耗时
        pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        //输入源点云和目标点云
        icp.setInputSource(origin_src_nonan);
        icp.setInputTarget(origin_tgt_nonan);
        //Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
        icp.setMaxCorrespondenceDistance(0.04);
        // 最大迭代次数
        icp.setMaximumIterations(50);
        // 两次变化矩阵之间的差值
        icp.setTransformationEpsilon(1e-10);
    
        // 均方误差,sac_trans为粗配准得到的刚体矩阵
        icp.setEuclideanFitnessEpsilon(0.2);
        icp.align(*icp_result, transformation_matrix);
    
        std::cout << "ICP has converged:" << icp.hasConverged()
                  << " score: " << icp.getFitnessScore() << std::endl;
        Eigen::Matrix4f icp_trans;
        icp_trans = icp.getFinalTransformation();
        //cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;
        std::cout << icp_trans << endl;
        //使用创建的变换对未过滤的输入点云进行变换
        pcl::transformPointCloud(*origin_src_nonan, *icp_result, icp_trans);
    
        std::cout <<icp.transformation_epsilon_;//输出最后一次迭代的迭代误差

**文末福利：看到最后的才有哦，我的所有头文件。不懂，就全添加，保证不会错。**

#pragma execution_character_set("utf-8")
    
    #include "mainwindow.h"
    #include <QApplication>
    #include <QCoreApplication>
    
    #include <pcl/io/pcd_io.h>
    #include <pcl/io/ply_io.h>
    #include <pcl/io/png_io.h>
    #include <pcl/range_image/bearing_angle_image.h>
    
    
    #include <pcl/impl/point_types.hpp>
    #include <pcl/console/parse.h>
    #include <pcl/common/transforms.h>
    
    #include <pcl/registration/ia_ransac.h>
    #include <pcl/point_types.h>
    #include <pcl/point_cloud.h>
    #include <pcl/features/normal_3d.h>
    #include <pcl/features/fpfh.h>
    #include <pcl/search/kdtree.h>
    #include <pcl/io/pcd_io.h>
    #include <pcl/filters/voxel_grid.h>
    #include <pcl/filters/filter.h>
    #include <pcl/registration/icp.h>
    
    //引入vtk进行点云三维显示
    #include <pcl/visualization/pcl_visualizer.h>
    
    #include <time.h>
    #include <pcl/kdtree/kdtree_flann.h>
    
    #include <iostream>
    #include <qdebug.h>
    
    #include<opencv2/highgui/highgui.hpp>
    #include<opencv2/features2d/features2d.hpp>
    #include<opencv2/xfeatures2d/nonfree.hpp>
    #include <opencv2/opencv.hpp>
    #include <opencv2/face.hpp>
    #include"opencv2/xfeatures2d.hpp"
    
    #include <opencv2/core.hpp>
    #include <opencv2/imgproc.hpp>
    
    #include <vtkSmartPointer.h>
    #include <vtkLineSource.h>
    #include <vtkPolyData.h>
    #include <vtkPolyDataMapper.h>
    #include <vtkActor.h>
    #include <vtkProperty.h>
    #include <vtkRenderWindow.h>
    #include <vtkRenderer.h>
    #include <vtkRenderWindowInteractor.h>
    #include <vtkLine.h>
    
    #include "vtkAutoInit.h"
    VTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL
    VTK_MODULE_INIT(vtkInteractionStyle);
    
    using namespace std;
    using namespace cv;
    using namespace cv::ml;
    using namespace face;
    using namespace cv::xfeatures2d;
    
    typedef pcl::PointXYZ PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    using pcl::NormalEstimation;
    using pcl::search::KdTree;
    using namespace pcl;
    using namespace pcl::io;

[PCL_VTK_windows]: https://blog.csdn.net/kissgoodbye2012/article/details/103644297
[PCL]: https://blog.csdn.net/kissgoodbye2012/article/details/103657487
[PCL_VTK]: https://market.m.taobao.com/app/idleFish-F2e/widle-taobao-rax/page-detail?wh_weex=true&wx_navbar_transparent=true&id=609983409501&ut_sk=1.AAhiIP37iQwDANlHHj81FWR6_21407387_1577024438216.Copy.detail.609983409501.1030106470&forceFlush=1
[qt]: https://market.m.taobao.com/app/idleFish-F2e/widle-taobao-rax/page-detail?wh_weex=true&wx_navbar_transparent=true&id=610270833284&ut_sk=1.AAhiIP37iQwDANlHHj81FWR6_21407387_1577024438216.Copy.detail.610270833284.1030106470&forceFlush=1
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20191222215352220.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg==,size_16,color_FFFFFF,t_70