//ComputeBuildingNormals
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/ModelCoefficients.h>//模型系数头文件
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>//基于采样一致性分割的类的头文件
#include <pcl/filters/extract_indices.h>
//pcl::ModelCoefficients::Ptr cofficients (new pcl::ModelCoefficients);//模型系数
//pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
using namespace std;
typedef pcl::PointXYZ PointT;
void SegBuilding(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,string filename);//定义一个函数，对输入点云进行分割
int
main (int argc, char** argv)
{
if(argc<2){
cout<<"Error!"<<endl;
return -1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile(argv[1],*cloud_in);
std::cout<<"cloud_in: "<<cloud_in->size()<<endl;
std::cout<<"fileds: "<<pcl::getFieldsList(*cloud_in)<<std::endl;
// 对点云重采样
pcl::search::KdTree<PointT>::Ptr treeSampling (new pcl::search::KdTree<PointT>);
pcl::PointCloud<PointT> mls_point;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_smoothed;
pcl::MovingLeastSquares<PointT,PointT> mls;
mls.setComputeNormals(false);
mls.setInputCloud(cloud_in);
mls.setPolynomialOrder(2);
mls.setPolynomialFit(false);
mls.setSearchMethod(treeSampling);
mls.setSearchRadius(0.05);
mls.process(mls_point);
// 输出重采样结果
cloud_smoothed = mls_point.makeShared();
ostringstream oss;
oss<<"Smoothed_"<<argv[1];
std::cout<<oss.str()<<" |size: "<<cloud_smoothed->size() <<std::endl;
std::cout<<oss.str()<<" |fields: "<<pcl::getFieldsList(mls_point)<<std::endl;
//save cloud_smoothed
pcl::io::savePCDFileASCII(oss.str(),*cloud_smoothed);
// 法线估计
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normalEstimation;
normalEstimation.setInputCloud(cloud_smoothed);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
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()<<"\n"<<"normals fields: "<<pcl::getFieldsList(*normals)<<std::endl;
oss.str("");
oss<<"NormalsOnly_"<<argv[1];
pcl::io::savePCDFileASCII(oss.str(),*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;
// oss.str("");
// oss<<"Normals_"<<argv[1];
// pcl::io::savePCDFileASCII(oss.str(),*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);
//可视化
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);
int v4(0);
viewer->setBackgroundColor(0,0,0,v4);
// viewer->addPolylineFromPolygonMesh(triangles,"GPTriangle");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(cloud_smoothed, 0,200,0);
//平面分割
SegBuilding(cloud_smoothed,argv[1]);//
cout<<"Final cloud_smoothed: "<<cloud_smoothed->size()<<endl;
//保存最后的剩下的点云
oss.str("");
oss<<"Remain_"<<argv[1];
pcl::io::savePCDFileASCII(oss.str(),*cloud_smoothed);
cout<<oss.str()<<" Saved!"<<endl;
// viewer->addText("cloud_filtered_out",0,0,"cloud_filtered_out",v4);
//viewer->addCoordinateSystem();
viewer->spin();
return 0;
}
void SegBuilding(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, string filename) {
cout<<"\nSegBuilding\n------------------------------------------------------"<<endl;
pcl::PCDWriter writer;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); //创建一个PointIndices结构体指针
// 创建分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 可选
seg.setOptimizeCoefficients(true); //设置对估计的模型做优化处理
// 必选
seg.setModelType(pcl::SACMODEL_PLANE);//设置分割模型类别
seg.setMethodType(pcl::SAC_RANSAC);//设置使用那个随机参数估计方法
seg.setMaxIterations(1000);//迭代次数
seg.setDistanceThreshold(0.045);//设置是否为模型内点的距离阈值
// 创建滤波器对象
pcl::ExtractIndices<pcl::PointXYZ> extract;
int i = 0, nr_points = (int) cloud_in->points.size();
cout << "All Point: " << cloud_in->size() <<"\n--------------------"<< endl;
double PointNumber = cloud_in->size();
// 当还多于30%原始点云数据时
while (cloud_in->points.size() > 0.1 * nr_points) {
// 从余下的点云中分割最大平面组成部分
seg.setInputCloud(cloud_in);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// 分离内层
extract.setInputCloud(cloud_in);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_p);//将分割点云根据inliers提取到cloud_p中
PointNumber = PointNumber - cloud_p->size();
std::cerr << "after cloud_filtered: " << PointNumber << std::endl;//剩余的点云
//保存
std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height
<< " data points." << std::endl;
std::stringstream ss;
ss << "Plane_" << i <<"_"<< filename; //对每一次的提取都进行了文件保存
writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);
cout<<ss.str()<<" saved!"<<endl;
std::cerr << "----------------------------------" << std::endl;
// 再次创建滤波器对象
extract.setNegative(true);//提取外层
extract.filter(*cloud_f);//将外层的提取结果保存到cloud_f
cloud_in.swap(cloud_f);//经cloud_filtered与cloud_f交换
i++;
}
}
效果如图：
