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

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

PCL系列文章

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

第二篇，PCL的使用

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

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

---

目录

第一、导入ply文件

第二、点云可视化

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

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

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

第六、去除NAN点

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

第八、保存点云为ply格式

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

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

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

第十二、icp进行精确匹配

---

第一、导入ply文件

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

#include 
#include 
#include 
 
   //导入ply格式的点云
    pcl::PointCloud::Ptr cloud_src_o(new pcl::PointCloud);
    if (pcl::io::loadPLYFile("./dragonStandRight_24.ply", *cloud_src_o) == -1) //* load the file
    {
        return (-1);
    }else {
        //成功导入
        qDebug()<<"src loads ok!";
        //        for(int i=0;isize();i++)
        //        {
        //            qDebug()<<"cloud_src_o点坐标x"<points[i].PointXYZ::x;
        //            qDebug()<<"cloud_src_o点坐标y"<points[i].PointXYZ::y;
        //            qDebug()<<"cloud_src_o点坐标z"<points[i].PointXYZ::z;
        //        }
    }

第二、点云可视化

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

首先，定义PCLVisualizer类的对象；

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

再添加点的数据；

最后进行显示。

//引入vtk进行点云三维显示
#include 

//原始点云和目标点云可视化
    pcl::visualization::PCLVisualizer viewer00("Visualization of original and target point cloud");
    viewer00.setBackgroundColor(255,255,255);
    pcl::visualization::PointCloudColorHandlerCustom single_color(cloud_src_o, 0, 255, 0);
    viewer00.addPointCloud(cloud_src_o, single_color, "sample cloud");
    pcl::visualization::PointCloudColorHandlerCustom single_color2(cloud_tgt_o, 255, 0, 0);
    viewer00.addPointCloud(cloud_tgt_o, single_color2, "sample cloud2");
    while (!viewer00.wasStopped())
    {
        viewer00.spinOnce(1);
    }

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

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

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

#include 
#include 

//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的宽度数："<(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 src_matchpoint_Index;
    vector tgt_matchpoint_Index;

    for(int p=0;p

第六、去除NAN点

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

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

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

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

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

//src_cloudout和tgt_cloudout已经是一一对应啦
    pcl::PointCloud::Ptr src_cloudOut(new pcl::PointCloud);
    pcl::copyPointCloud(*cloud_src_o, src_matchpoint_Index, *src_cloudOut);
    pcl::PointCloud::Ptr tgt_cloudOut(new pcl::PointCloud);
    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 lines =vtkSmartPointer::New();//连线集合
    for(int i=0;isize();i++)
    {
        vtkSmartPointer line = vtkSmartPointer::New();
        //第一个参数是序号，第二个参数是点的序号
        line->GetPointIds()->SetId(0,i);
        line->GetPointIds()->SetId(1,true_tgt_cloudOut->size()+i);
        lines->InsertNextCell(line);
    }

    //存储点云和连线的数据集
    vtkSmartPointer pdata =vtkSmartPointer::New();
    //添加所有的点
    pdata->SetPoints(pts);
    //添加所有的线
    pdata->SetLines(lines);

    //vtk中绘图
    vtkPolyDataMapper *mapper = vtkPolyDataMapper::New();
    mapper->SetInputData(pdata);

    vtkSmartPointer actor =vtkSmartPointer::New();
    actor->SetMapper(mapper);
    actor->GetProperty()->SetColor(1, 0, 0);
    actor->GetProperty()->SetLineWidth(4);

    vtkSmartPointer renderer =vtkSmartPointer::New();
    vtkSmartPointer renderWindow = vtkSmartPointer::New();
    renderWindow->AddRenderer(renderer);
    vtkSmartPointer renderWindowInteractor =vtkSmartPointer::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 TESVD;
    pcl::registration::TransformationEstimationSVD::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::Ptr icp_result(new pcl::PointCloud);
    pcl::IterativeClosestPoint 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"<

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

#pragma execution_character_set("utf-8")

#include "mainwindow.h"
#include 
#include 

#include 
#include 
#include 
#include 


#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//引入vtk进行点云三维显示
#include 

#include 
#include 

#include 
#include 

#include
#include
#include
#include 
#include 
#include"opencv2/xfeatures2d.hpp"

#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#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 PointCloud;
using pcl::NormalEstimation;
using pcl::search::KdTree;
using namespace pcl;
using namespace pcl::io;