PCL點雲使用貪婪三角化進行曲面重構

一、PCL點雲平滑和法線估計

題目：https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486705&idx=1&sn=ca333d7bb12b7c226270e98d0003a789&chksm=97d7e966a0a06070a8dba605966016d227d7a6cad786498070d9e1b8cea8747470a4840257fd&scene=21#wechat_redirect

/****************************
 * 題目：給定一個融合後的點雲，已經對其進行下采樣和濾波（代碼已給）。
 * 請對其進行平滑（輸出結果），然後計算法線，并講法線顯示在平滑後的點雲上（提供截圖）。
 *
* 本程式學習目标：
 * 熟悉PCL的平滑、法線計算、顯示，為網格化做鋪墊。
 *
 * 公衆号：計算機視覺life。釋出于公衆号旗下知識星球：從零開始學習SLAM
 * 時間：2018.12
****************************/
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>

typedef pcl::PointXYZRGB PointT;

int main(int argc, char** argv)
{

	// Load input file
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_downSampled(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_smoothed(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("./fusedCloud.pcd", *cloud) == -1)
    {
        cout << "點雲資料讀取失敗！" << endl;
    }

    std::cout << "Orginal points number: " << cloud->points.size() << std::endl;

    // 下采樣，同時保持點雲形狀特征
    pcl::VoxelGrid<PointT> downSampled;  //建立濾波對象
    downSampled.setInputCloud (cloud);            //設定需要過濾的點雲給濾波對象
    downSampled.setLeafSize (0.01f, 0.01f, 0.01f);  //設定濾波時建立的體素體積為1cm的立方體
    downSampled.filter (*cloud_downSampled);           //執行濾波處理，存儲輸出
    std::cout<<"cloud_downSampled: " << cloud_downSampled->size()<<std::endl;

    pcl::io::savePCDFile ("./downsampledPC.pcd", *cloud_downSampled);

	// 統計濾波
	pcl::StatisticalOutlierRemoval<PointT> statisOutlierRemoval;       //建立濾波器對象
    statisOutlierRemoval.setInputCloud (cloud_downSampled);            //設定待濾波的點雲
    statisOutlierRemoval.setMeanK (50);                                //設定在進行統計時考慮查詢點臨近點數
    statisOutlierRemoval.setStddevMulThresh (3.0);                     //設定判斷是否為離群點的閥值:均值+1.0*标準差
    statisOutlierRemoval.filter (*cloud_filtered);                     //濾波結果存儲到cloud_filtered

    std::cout << "cloud_filtered: " << cloud_filtered->size()<<std::endl;
    pcl::io::savePCDFile ("./filteredPC.pcd", *cloud_filtered);
	// ----------------------開始你的代碼--------------------------//
	// 請參考PCL官網實作以下功能
	// 對點雲重采樣
	pcl::search::KdTree<PointT>::Ptr treeSampling (new pcl::search::KdTree<PointT>);
	pcl::PointCloud<PointT> mls_point;
	pcl::MovingLeastSquares<PointT,PointT> mls;
	mls.setComputeNormals(false);
	mls.setInputCloud(cloud_filtered);
	mls.setPolynomialOrder(2);
	mls.setPolynomialFit(false);
	mls.setSearchMethod(treeSampling);
	mls.setSearchRadius(0.05);
	mls.process(mls_point);


    // 輸出重采樣結果
    cloud_smoothed = mls_point.makeShared();
    std::cout<<"cloud_smoothed: "<<cloud_smoothed->size() <<std::endl;

    //save cloud_smoothed
    pcl::io::savePCDFileASCII("cloud_smoothed.pcd",*cloud_smoothed);
 

	// 法線估計
	pcl::NormalEstimation<PointT,pcl::Normal> normalEstimation;
	normalEstimation.setInputCloud(cloud_smoothed);
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
	normalEstimation.setSearchMethod(tree);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	normalEstimation.setKSearch(10);
	normalEstimation.compute(*normals);
	std::cout<<"normals: "<<normals->size()<<", "<<"normals fields: "<<pcl::getFieldsList(*normals)<<std::endl;
	pcl::io::savePCDFileASCII("normals.pcd",*normals);

	// ----------------------結束你的代碼--------------------------//
	// 顯示結果
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("PCL Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_smoothed);//color
    viewer->addPointCloud<PointT> (cloud_smoothed, rgb, "smooth cloud");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "smooth cloud");
    viewer->addPointCloudNormals<PointT, pcl::Normal> (cloud_smoothed, normals, 10, 0.1, "normals");

    viewer->initCameraParameters ();

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    return 0;
}

1、控制法線顯示的數目：

viewer->addPointCloudNormals<PointT, pcl::Normal> (cloud_smoothed, normals, 10, 0.1, "normals");

10，就是每10個法線顯示一個。

0.1，就是每個法線的長度。

2、擷取PointCloud的Fields

pcl::getFieldsList(*normals)

二、貪婪三角化投影曲面重建

計算流程：點雲輸入 --> 下采樣 --> 統計濾波去除離群點 --> mls移動最小二乘法進行平滑處理 --> 對平滑後的點雲進行法線估計(有向點雲) --> 将法線和平滑後的點雲的Fields拼接在一起 --> 貪婪三角化 -->顯示輸出

/****************************
 * 題目：給定一個融合後的點雲，已經對其進行下采樣和濾波（代碼已給）。
 * 請對其進行平滑（輸出結果），然後計算法線，并講法線顯示在平滑後的點雲上（提供截圖）。
 *
* 本程式學習目标：
 * 熟悉PCL的平滑、法線計算、顯示，為網格化做鋪墊。
 *
 * 公衆号：計算機視覺life。釋出于公衆号旗下知識星球：從零開始學習SLAM
 * 時間：2018.12
****************************/
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>

#include <pcl/surface/gp3.h>

typedef pcl::PointXYZ PointT;

int main(int argc, char** argv)
{

	// Load input file
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_downSampled(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_smoothed(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("./fusedCloud.pcd", *cloud) == -1)
    {
        cout << "點雲資料讀取失敗！" << endl;
    }

    std::cout << "Orginal points number: " << cloud->points.size() << std::endl;

    // 下采樣，同時保持點雲形狀特征
    pcl::VoxelGrid<PointT> downSampled;  //建立濾波對象
    downSampled.setInputCloud (cloud);            //設定需要過濾的點雲給濾波對象
    downSampled.setLeafSize (0.01f, 0.01f, 0.01f);  //設定濾波時建立的體素體積為1cm的立方體
    downSampled.filter (*cloud_downSampled);           //執行濾波處理，存儲輸出
    std::cout<<"cloud_downSampled: " << cloud_downSampled->size()<<std::endl;

    pcl::io::savePCDFile ("./downsampledPC.pcd", *cloud_downSampled);

	// 統計濾波
	pcl::StatisticalOutlierRemoval<PointT> statisOutlierRemoval;       //建立濾波器對象
    statisOutlierRemoval.setInputCloud (cloud_downSampled);            //設定待濾波的點雲
    statisOutlierRemoval.setMeanK (50);                                //設定在進行統計時考慮查詢點臨近點數
    statisOutlierRemoval.setStddevMulThresh (3.0);                     //設定判斷是否為離群點的閥值:均值+1.0*标準差
    statisOutlierRemoval.filter (*cloud_filtered);                     //濾波結果存儲到cloud_filtered

    std::cout << "cloud_statical_filtered: " << cloud_filtered->size()<<std::endl;
    pcl::io::savePCDFile ("./filteredPC.pcd", *cloud_filtered);
	// ----------------------開始你的代碼--------------------------//
	// 請參考PCL官網實作以下功能
	// 對點雲重采樣
	pcl::search::KdTree<PointT>::Ptr treeSampling (new pcl::search::KdTree<PointT>);
	pcl::PointCloud<PointT> mls_point;
	pcl::MovingLeastSquares<PointT,PointT> mls;
	mls.setComputeNormals(false);
	mls.setInputCloud(cloud_filtered);
	mls.setPolynomialOrder(2);
	mls.setPolynomialFit(false);
	mls.setSearchMethod(treeSampling);
	mls.setSearchRadius(0.05);
	mls.process(mls_point);


    // 輸出重采樣結果
    cloud_smoothed = mls_point.makeShared();
    std::cout<<"cloud_smoothed: "<<cloud_smoothed->size() <<std::endl;

    //save cloud_smoothed
    pcl::io::savePCDFileASCII("cloud_smoothed.pcd",*cloud_smoothed);
 

	// 法線估計
	pcl::NormalEstimation<PointT,pcl::Normal> normalEstimation;
	normalEstimation.setInputCloud(cloud_smoothed);
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
	normalEstimation.setSearchMethod(tree);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	normalEstimation.setKSearch(10);
	normalEstimation.compute(*normals);
	std::cout<<"normals: "<<normals->size()<<", "<<"normals fields: "<<pcl::getFieldsList(*normals)<<std::endl;
	pcl::io::savePCDFileASCII("normals.pcd",*normals);

	//Triangle Projection
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields(*cloud_smoothed,*normals,*cloud_with_normals);
	std::cout<<"cloud_with_normals fields: "<<pcl::getFieldsList(*cloud_with_normals)<<std::endl;

	pcl::io::savePCDFileASCII("cloud_with_normals.pcd",*cloud_with_normals);

	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud(cloud_with_normals);

	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	pcl::PolygonMesh triangles;

	gp3.setSearchRadius(0.1);
	gp3.setMu(2.5);
	gp3.setMaximumNearestNeighbors(52);

	gp3.setMaximumAngle(2*M_PI/3);
	gp3.setMinimumAngle(M_PI/18);

	gp3.setMaximumSurfaceAngle(M_PI/4);
	gp3.setNormalConsistency(false);

	gp3.setInputCloud(cloud_with_normals);
	gp3.setSearchMethod(tree2);
	gp3.reconstruct(triangles);


	// ----------------------結束你的代碼--------------------------//
	// 顯示結果
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("PCL Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
/*
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_smoothed);//color
    viewer->addPointCloud<PointT> (cloud_smoothed, rgb, "smooth cloud");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "smooth cloud");
    viewer->addPointCloudNormals<PointT, pcl::Normal> (cloud_smoothed, normals, 10, 0.1, "normals");
*/
    viewer->addPolygonMesh(triangles,"GPTriangle");
    viewer->addText("GPTriangle",0,0,"GPTriangle");
    viewer->initCameraParameters ();

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    return 0;
}

參考：https://cloud.tencent.com/developer/article/1436518

可視化：

PCL點雲使用貪婪三角化進行曲面重構

重建效果圖2：

PCL點雲使用貪婪三角化進行曲面重構

幾個需要注意的地方：

1、貪婪三角化算法必須要求點雲是平滑的，且密度變化均勻（這也是上一步為什麼要做一個平滑處理）。

2、貪婪投影三角化要求輸入的點雲必須是有向點雲，是以對平滑後的點雲進行法向量估計，并且将其Fields拼接在一起

3、拼接Fields的時候函數

pcl::concatenateFields

隻能接受PointXYZ類型的點雲資料，對于平滑後的帶有RGB資訊的點雲要将其以PointXYZ的類型讀入。

4、重建結果顯示是函數

viewer->addPolygonMesh(triangles,"GPTriangle");

PCL點雲使用貪婪三角化進行曲面重構

繼續閱讀

PCL - PCLBase代碼研讀（二）- PCLBase實現PCL - PCLBase代碼研讀（二）- PCLBase實現

PCL - MLS代碼研讀（一）- MLS測試前言測試曲面重建測試SAMPLE_LOCAL_PLANE上採樣方法測試VOXEL_GRID_DILATION上採樣方法主程式MLS module結構

PCL - MLS代碼研讀（八）- MovingLeastSquares

PCL - MLS代碼研讀（十二）- addProjectedPointNormal及copyMissingFields函數

PCL - ICP代碼研讀（六） - IterativeClosestPoint架構

PCL - ICP代碼研讀（十三） - CorrespondenceEstimationBase實現

PCL點雲庫——PCA粗配準

《4-Points Congruent Sets for Robust Pairwise Surface Registration》論文研讀

PCL點雲學習（1）——pcd檔案的生成和讀取

PCL1.6.0學習筆記（一）

ROS 中bag 包

66.[PCL]PCD檔案格式簡介PCD版本

Matlab讀取.pcd格式HDmap環境

velodyne 擷取32線 csv 轉換成 pcd 檔案32線 csv 轉換成 pcd 檔案 C++

PCL 讀取PCD格式并可視化

PCL環境配置--win10+VS2019+PCL1.11.1