一、算法原理
見《點雲庫PCL從入門到精通》P164頁
二、代碼實現
#include <iostream>
#include <vector>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read("pig.pcd",*cloud);
//---------爲了構造2D封閉多邊形,首先輸入2D平面點雲,這些平面點是2D封閉多邊形的頂點。
pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
//boundingbox_ptr->push_back(pcl::PointXYZ(0.1, 0.1, 0));//四個頂點即可
boundingbox_ptr->push_back(pcl::PointXYZ(0.1, -0.1,0 ));
boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, 0.1,0 ));
boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, -0.1,0 ));
boundingbox_ptr->push_back(pcl::PointXYZ(0.15, 0.1,0 ));
//---------對上述2D平面點構造凸包-----------------------
pcl::ConvexHull<pcl::PointXYZ> hull;//創建凸包對象
hull.setInputCloud(boundingbox_ptr);//設置輸入點雲
hull.setDimension(2);//設置凸包維度
vector<pcl::Vertices> polygons;//設置pcl:Vertices類型的向量,用於保存凸包頂點
pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);//該點雲用於描述凸包形狀
hull.reconstruct(*surface_hull, polygons);//計算2D凸包結果
//---------創建CropHull對象,濾波得到2D封閉凸包範圍內的點雲,此處的維度需要與輸入凸包維度一致
pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
pcl::CropHull<pcl::PointXYZ> bb_filter;//創建CropHull對象
bb_filter.setDim(2);//設置維度
bb_filter.setInputCloud(cloud);//設置需要濾波的點雲
bb_filter.setHullIndices(polygons);//輸入封閉多邊形的頂點
bb_filter.setHullCloud(surface_hull);//輸入封閉多邊形的形狀
bb_filter.filter(*objects);//執行CropHull濾波,儲存結果到objects
cout << objects->size() << endl;
//---------------可視化,從左到右依次是原始點雲,封閉2D多邊形凸包結果,CropHull濾波結果------------
boost::shared_ptr<pcl::visualization::PCLVisualizer> for_visualizer_v (new pcl::visualization::PCLVisualizer ("crophull display"));
//for_visualizer_v->setBackgroundColor(255,255,255);
//原始點雲
int v1(0);
for_visualizer_v->createViewPort (0.0, 0.0, 0.33, 1, v1);
for_visualizer_v->setBackgroundColor (0, 255, 0, v1);
for_visualizer_v->addPointCloud (cloud,"cloud",v1);
for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,0,0,"cloud");
for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"cloud");
for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull,0,.069*255,0.2*255,"backview_hull_polyline1",v1);
//封閉2D多邊形凸包結果
int v2(0);
for_visualizer_v->createViewPort (0.33, 0.0, 0.66, 1, v2);
for_visualizer_v->setBackgroundColor (0, 0, 255, v2);
for_visualizer_v->addPointCloud (surface_hull,"surface_hull",v2);
for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,0,0,"surface_hull");
for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"surface_hull");
for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull,0,.069*255,0.2*255,"backview_hull_polyline",v2);
//CropHull濾波結果
int v3(0);
for_visualizer_v->createViewPort (0.66, 0.0, 1, 1, v3);
for_visualizer_v->setBackgroundColor (0, 255, 0, v3);
for_visualizer_v->addPointCloud (objects,"objects",v3);
for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,0,0,"objects");
for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"objects");
while (!for_visualizer_v->wasStopped())
{
for_visualizer_v->spinOnce(1000);
}
return 0;
}
三、結果展示
從左到右依次是原始點雲,封閉2D多邊形凸包結果,CropHull濾波結果