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

原創 kissgoodbye2012 2020-04-21 08:04

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

 

 

發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章

PCL系列文章——第一篇，PCL+VTK在windows下的安裝

kissgoodbye2012 2020-02-24 10:22:26

第一章 工程師數學算法庫之線性代數矩陣運算庫Eigen的使用

kissgoodbye2012 2020-02-24 10:22:26

PCL系列文章——第一篇，PCL+VTK在windows下的安裝

kissgoodbye2012 2020-02-24 10:22:26

第一章 工程師數學算法庫之線性代數矩陣運算庫Eigen的使用

kissgoodbye2012 2020-02-24 10:22:26

梯度下降法總結

kissgoodbye2012 2018-08-27 05:56:40