#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
void visualize_pcd(PointCloud::Ptr pcd_src1,
PointCloud::Ptr pcd_src2,
PointCloud::Ptr pcd_final)
{
int vp_1, vp_2;
// Create a PCLVisualizer object
pcl::visualization::PCLVisualizer viewer("PCL 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 src1_h (pcd_src1, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ src2_h (pcd_src2, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ final_h (pcd_final, 0, 255, 0);
viewer.addPointCloud (pcd_src1, src1_h, "vp1_src1", vp_1);
viewer.addPointCloud (pcd_src2, src2_h, "vp1_src2", vp_1);
viewer.addPointCloud (pcd_final, final_h, "vp1_final", vp_2);
viewer.spin();
}
int
main (int argc, char** argv)
{
PointCloud::Ptr cloud_final (new PointCloud);
PointCloud::Ptr cloud_src (new PointCloud);//将长的转成短的//读取点云数据
pcl::io::loadPCDFile ("1.pcd",*cloud_src);
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne_src;
ne_src.setInputCloud(cloud_src);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
ne_src.setSearchMethod(tree_src);
pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
ne_src.setRadiusSearch(0.03);
//ne_src.setKSearch(20);二选一
ne_src.compute(*cloud_src_normals);
PointCloud::Ptr cloud_tgt (new PointCloud);
pcl::io::loadPCDFile ("2.pcd",*cloud_tgt);
pcl::NormalEstimation<pcl::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::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
//ne_tgt.setKSearch(20);
ne_tgt.setRadiusSearch(0.03);
//compute normal to cloud_normals
ne_tgt.compute(*cloud_tgt_normals);
//fpfh estimation
pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_src;
fpfh_src.setInputCloud(cloud_src);
fpfh_src.setInputNormals(cloud_src_normals);
pcl::search::KdTree<PointT>::Ptr tree_src_fpfh (new pcl::search::KdTree<PointT>);
fpfh_src.setSearchMethod(tree_src_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_src.setRadiusSearch(0.05);
//fpfh_src.setKSearch(20);
fpfh_src.compute(*fpfhs_src);
pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_tgt;
fpfh_tgt.setInputCloud(cloud_tgt);
fpfh_tgt.setInputNormals(cloud_tgt_normals);
pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh (new pcl::search::KdTree<PointT>);
fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_tgt.setRadiusSearch(0.05);
//fpfh_tgt.setKSearch(20);
fpfh_tgt.compute(*fpfhs_tgt);
//sample consensus initial alignment
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputCloud(cloud_src);
scia.setInputTarget(cloud_tgt);
scia.setSourceFeatures(fpfhs_src);
scia.setTargetFeatures(fpfhs_tgt);
//scia.setMinSampleDistance(1);
//scia.setNumberOfSamples(2);
//scia.setCorrespondenceRandomness(20);
PointCloud::Ptr cloud_result_initial (new PointCloud);
scia.align(*cloud_result_initial);
visualize_pcd(cloud_src,cloud_tgt,cloud_result_initial);
std::cout <<"totals time is:"<<clock()<<std::endl;
return (0);
}