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

最新推荐文章于 2024-05-26 19:45:00 发布

kissgoodbye2012

最新推荐文章于 2024-05-26 19:45:00 发布

阅读量1.8k

点赞数

分类专栏： 004 解法算法文章标签： PCL 函数使用点云配准

本文链接：https://blog.csdn.net/kissgoodbye2012/article/details/103657487

版权

004 解法算法专栏收录该内容

15 篇文章 3 订阅

订阅专栏

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

PCL系列文章

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

第二篇，PCL的使用

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

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

第一、导入ply文件

第二、点云可视化

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

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

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

第十二、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图呢？如下定义。我们把三维点云转换为二维图像，提取特征点，匹配特征点，然后将匹配的特征点重新转换为三维的匹配点，就可以得到匹配的三维点云对。

#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;

kissgoodbye2012

关注

0
点赞
踩
20

收藏

觉得还不错? 一键收藏
打赏
1
评论
PCL系列文章——第二篇，PCL的使用

PCL是点云处理一个非常强大的库，可以完成多种点云的操作算法；而VTK又是三维点云显示以及三维重建非常有用的库。虽然只会调库难免让人绝对低端，但这些库的功能也是真的强大。PCL系列文章第一篇，PCL+VTK在windows下的安装第二篇，PCL的使用如果需要PCL+VTK的安装文件，咸鱼链接：如果需要三维点云配准的qt程序，咸鱼链接目录第一、导入ply文件第二、...
复制链接

扫一扫