点云配准，下采样，SAC ICP配准等

#include
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

#include
#include
#include
#include <Eigen/Core>

#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.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>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
PointCloud::Ptr pcd_tgt,
PointCloud::Ptr pcd_final)
{
//int vp_1, vp_2;
// Create a PCLVisualizer object
pcl::visualization::PCLVisualizer viewer(“registration Viewer”);
//viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
// viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ src_h (pcd_src, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ tgt_h (pcd_tgt, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ final_h (pcd_final, 0, 0, 255);
viewer.addPointCloud (pcd_src, src_h, “source cloud”);
viewer.addPointCloud (pcd_tgt, tgt_h, “tgt cloud”);
viewer.addPointCloud (pcd_final, final_h, “final cloud”);
//viewer.addCoordinateSystem(1.0);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}

int
main (int argc, char** argv)
{
//read pcd dian yun shu ju du dao cloud_camera
pcl::PointCloudpcl::PointXYZ::Ptr cloud_camera (new pcl::PointCloudpcl::PointXYZ);

if (pcl::io::loadPCDFilepcl::PointXYZ ("/home/tao/workshop/Date/testScanner_point_cloud.pcd", cloud_camera) == -1) // load the file
{
PCL_ERROR (“Couldn’t read file test_pcd.pcd \n”);
return (-1);
}
std::cout << "Loaded "
<< cloud_camera->width * cloud_camera->height
<< " data points from test_pcd.pcd with the following fields: "
<< std::endl;

// shuang bian lu bo
pcl::PointCloudpcl::PointXYZ::Ptr cloud_PassThrough (new pcl::PointCloudpcl::PointXYZ);

// Create the filtering object cu lu bo

pcl::PassThroughpcl::PointXYZ pass;
pass.setInputCloud (cloud_camera);
pass.setFilterFieldName (“z”);
pass.setFilterLimits (0.0, 1220.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_PassThrough);

pass.setInputCloud (cloud_PassThrough);
pass.setFilterFieldName (“x”);
pass.setFilterLimits (-150, 120.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_PassThrough);

// Create the filtering object xi lu bo
pcl::PointCloudpcl::PointXYZ::Ptr cloud_remove (new pcl::PointCloudpcl::PointXYZ);
pcl::StatisticalOutlierRemovalpcl::PointXYZ sor;
sor.setInputCloud (cloud_PassThrough);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_remove);

//jiang cai yang
// Create the filtering object
pcl::PointCloudpcl::PointXYZ::Ptr cloud_filtered (new pcl::PointCloudpcl::PointXYZ);
pcl::VoxelGridpcl::PointXYZ sor1;
sor1.setInputCloud (cloud_remove);
sor1.setLeafSize (1.0f,1.0f, 1.0f);
sor1.filter (*cloud_filtered);

//*****************************************************//

// Creating the KdTree object for the search method of the extraction
pcl::search::KdTreepcl::PointXYZ::Ptr tree (new pcl::search::KdTreepcl::PointXYZ);
tree->setInputCloud (cloud_filtered);

std::vectorpcl::PointIndices cluster_indices;
pcl::EuclideanClusterExtractionpcl::PointXYZ ec;
ec.setClusterTolerance (20); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);

int j = 0;
for (std::vectorpcl::PointIndices::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloudpcl::PointXYZ::Ptr cloud_cluster (new pcl::PointCloudpcl::PointXYZ);
for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->push_back ((*cloud_filtered)[pit]); //
cloud_cluster->width = cloud_cluster->size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;

std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
if(j==2){

PointCloud::Ptr cloud_tgt_o (new PointCloud);//目标点云
pcl::io::loadPCDFile ("/home/tao/workshop/Date/Model_Industry2.pcd",*cloud_tgt_o);

  //计算表面法线

pcl::NormalEstimationpcl::PointXYZ,pcl::Normal ne_src;
ne_src.setInputCloud(cloud_cluster);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
ne_src.setSearchMethod(tree_src);
pcl::PointCloudpcl::Normal::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
ne_src.setRadiusSearch(0.02);
ne_src.compute(*cloud_src_normals);

std::vector indices_tgt;
pcl::removeNaNFromPointCloud(*cloud_tgt_o,*cloud_tgt_o, indices_tgt);
std::cout<<“remove *cloud_tgt_o nan”<<endl;

pcl::VoxelGridpcl::PointXYZ voxel_grid_2;
voxel_grid_2.setLeafSize(0.01,0.01,0.01);
voxel_grid_2.setInputCloud(cloud_tgt_o);
PointCloud::Ptr cloud_tgt (new PointCloud);
voxel_grid_2.filter(*cloud_tgt);
std::cout<<"down size *cloud_tgt_o.pcd from "<<cloud_tgt_o->size()<<“to”<<cloud_tgt->size()<<endl;
pcl::io::savePCDFileASCII(“bunny_tgt_down.pcd”,*cloud_tgt);

pcl::NormalEstimationpcl::PointXYZ,pcl::Normal ne_tgt;
ne_tgt.setInputCloud(cloud_tgt);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
ne_tgt.setSearchMethod(tree_tgt);
pcl::PointCloudpcl::Normal::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
//ne_tgt.setKSearch(20);
ne_tgt.setRadiusSearch(0.02);
ne_tgt.compute(*cloud_tgt_normals);

//计算FPFH
pcl::FPFHEstimationpcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33 fpfh_src;
fpfh_src.setInputCloud(cloud_cluster);
fpfh_src.setInputNormals(cloud_src_normals);
pcl::search::KdTree::Ptr tree_src_fpfh (new pcl::search::KdTree);
fpfh_src.setSearchMethod(tree_src_fpfh);
pcl::PointCloudpcl::FPFHSignature33::Ptr fpfhs_src(new pcl::PointCloudpcl::FPFHSignature33());
fpfh_src.setRadiusSearch(0.05);
fpfh_src.compute(*fpfhs_src);
std::cout<<“compute *cloud_src fpfh”<<endl;

pcl::FPFHEstimationpcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33 fpfh_tgt;
fpfh_tgt.setInputCloud(cloud_tgt);
fpfh_tgt.setInputNormals(cloud_tgt_normals);
pcl::search::KdTree::Ptr tree_tgt_fpfh (new pcl::search::KdTree);
fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
pcl::PointCloudpcl::FPFHSignature33::Ptr fpfhs_tgt(new pcl::PointCloudpcl::FPFHSignature33());
fpfh_tgt.setRadiusSearch(0.05);
fpfh_tgt.compute(*fpfhs_tgt);
std::cout<<“compute *cloud_tgt fpfh”<<endl;

//SAC配准
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputSource(cloud_cluster);
scia.setInputTarget(cloud_tgt);
scia.setSourceFeatures(fpfhs_src);
scia.setTargetFeatures(fpfhs_tgt);
//scia.setMinSampleDistance(1);
//scia.setNumberOfSamples(2);
//scia.setCorrespondenceRandomness(20);
PointCloud::Ptr sac_result (new PointCloud);
scia.align(*sac_result);
std::cout <<“sac has converged:”<<scia.hasConverged()<<" score: "<<scia.getFitnessScore()<<endl;
Eigen::Matrix4f sac_trans;
sac_trans=scia.getFinalTransformation();
std::cout<<sac_trans<<endl;
pcl::io::savePCDFileASCII(“bunny_transformed_sac.pcd”,*sac_result);

//icp配准
PointCloud::Ptr icp_result (new PointCloud);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_cluster);
icp.setInputTarget(cloud_tgt_o);
//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);
// 均方误差
icp.setEuclideanFitnessEpsilon (0.2);
icp.align(*icp_result,sac_trans);

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(*cloud_cluster, *icp_result, icp_trans);
//保存转换的输入点云
pcl::io::savePCDFileASCII(“bunny_transformed_sac_ndt.pcd”, *icp_result);

//可视化
visualize_pcd(cloud_cluster,cloud_tgt_o,icp_result);
return (0);
}

}

//save pcd
// pcl::io::savePCDFileASCII ("/home/tao/workshop/Date/cloud_PassThrough.pcd", *cloud_cluster);// cun chu han shu

return (0);
}