三维重建基本流程，搭建一个龙的demo

于 2021-04-22 22:35:40 发布
阅读量1.2k 收藏 9
点赞数 1
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/layAlex/article/details/116033434
版权

最近在学习三维重建的一些知识，作为一个小白，从入门到完成一次重建过程是一个需要花一段比较长时间的过程，在网上搜索各种资料时，会因为资料不全，导致无法理清楚逻辑，从而导致入门困难。
我主要是通过书和网上的博客来学习的，如果有时间的话，建议看书，会更加深入的理解整个过程，我这里推荐经典的一本书《点云库PCL从入门到精通》。
这里我将按照我的理解尽可能用简洁的语言来讲清楚三维重建这件事情。

什么是三维重建

你已知一个事物或者一个场景从各个角度得到的三维点云片段，点云与点云之间存在部分重合，你需要做的就是通过某种方式将这些点云拼接在一起，这就是所谓的三维重建。

点云从哪里来

有人会问点云从哪里来，点云的来源有很多种方式，常见的是通过双目视觉，深度相机，结构光或者声纳等硬件设备来获取三维点云，这里不详细讲了

点云重建过程

当你拿到一系列点云时，你需要做以下步骤来进行点云拼接

  1. 采样处理，由于你拿到的点云是非常庞大的，所以你需要进行采样处理，减少计算量，这里我采用的是下体素采样。
  2. 法向量估计，两幅点云之所以能较为准确的拼接在一起，是因为两幅点云之间有共同的特征，这里选用的是法线特征，所以你需要估计法线特征
  3. 计算描述子，这里使用的是fpfh描述子，描述子是用来对匹配两幅点云的，根据法向量才能得到描述子，通过描述子才能知道两幅图的哪些点云是匹配的
  4. 特征点匹配，特征点匹配分为两步，第一步是粗配，使两个点云基本重合，这里用的方法是采样一致性粗配，然后通过icp方法精配，这样就能得到拼接完成的点云了，但是精度跟你设置的参数有关，主要是setMaxCorrespondenceDistance()方法设置最大匹配的距离，记住粗配时要设得尽量大一点，这样才能搜索到正确的匹配点，精配时需要设置小一点，否则会匹配失效（这是因为ICP只有在两个点云非常接近的时候才能使用，ICP算法没有用到特征点，而是直接根据点云与点云之间的最小距离来匹配点云，属于硬配，不了解ICP算法的同学可以去查一下相关资料）。
  5. 点云坐标变换，根据第四步你可以得到两点云之间的变换矩阵，然后如果以某一点云作为基准点云，那么你就可以得到其他点云与这个点云之间的变换矩阵，然后通过变换将所有点云旋转平移到统一坐标系下，即可得到重建完的点云图。

以上是点云坐标转换最基本的流程，适合初学者完成一个简单的demo

你需要做的一些前期工作

下面是代码，供大家参考，有任何问题可以在留言区给我留言~

//
///* 目标:对输入的model,scene计算.匹配关系,并估计位姿
//* 下采样->计算特征(pfh)->sac粗配->icp精配-》点云显示
//* 可视化点云以及关键点,并可视化其特征直方图
//*/

#include<string>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/point_cloud.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/pfh.h>
#include <pcl/features/shot_omp.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include<pcl/io/io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/io.h>
#include <time.h>
#include <iostream>
#include <pcl/keypoints/iss_3d.h>
#include <cstdlib>
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
#include <pcl/registration/sample_consensus_prerejective.h>   // pose estimate
#include <pcl/keypoints/sift_keypoint.h>   // shift关键点相关
#include <pcl/pcl_macros.h>
#include <pcl\features\fpfh.h>
#include <pcl\features\fpfh_omp.h>//omp加速计算

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


typedef pcl::PointXYZ PointType;
typedef pcl::Normal NormalType;

typedef pcl::SHOT352 DescriptorType;
float descr_rad_(3.0f);  // shot描述子的搜索半径


						 // sift相关
namespace pcl
{
	template<>
	struct SIFTKeypointFieldSelector<PointXYZ>
	{
		inline float
			operator () (const PointXYZ &p) const
		{
			return p.z;
		}
	};
}


//可视化对应关系
//input: model_keypoints, scene_keypoints, model_scene_corrs
void
visualize_corrs(pcl::PointCloud<pcl::PointXYZ>::Ptr model_keypoints, pcl::PointCloud<pcl::PointXYZ>::Ptr scene_keypoints,
	pcl::PointCloud<pcl::PointXYZ>::Ptr model, pcl::PointCloud<pcl::PointXYZ>::Ptr scene,
	pcl::CorrespondencesPtr model_scene_corrs)
{
	// 添加关键点
	pcl::visualization::PCLVisualizer viewer("corrs Viewer");
	viewer.setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> model_color(model, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> scene_color(scene, 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> model_keypoint_color(model_keypoints, 0, 255, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> scene_keypoint_color(scene_keypoints, 0, 0, 255);
	//    viewer.addPointCloud(model, model_color, "model");
	//    viewer.addPointCloud(scene, scene_color, "scene");
	viewer.addPointCloud(model_keypoints, model_keypoint_color, "model_keypoints");
	viewer.addPointCloud(scene_keypoints, scene_keypoint_color, "scene_keypoints");
	viewer.addPointCloud(model, model_color, "model");
	viewer.addPointCloud(scene, scene_color, "scene");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "model_keypoints");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scene_keypoints");

	viewer.addCorrespondences<pcl::PointXYZ>(model_keypoints, scene_keypoints, *model_scene_corrs);
	//    //添加对应关系
	//    int i=1;
	//    for(auto iter=model_scene_corrs->begin();iter!=model_scene_corrs->end();++iter)
	//    {
	//        std::stringstream ss_line;
	//        ss_line << "correspondence_line" << i ;
	//        i++;
	//        PointType& model_point = model_keypoints->at (iter->index_query);  // 从corrs中获取对应点
	//        PointType& scene_point = scene_keypoints->at (iter->index_match);
	//        viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 0, 0, ss_line.str ());
	//        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, ss_line.str ());   // 设置线宽
	//
	//
	//
	//    }

	// 显示
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

}


//采样
void filter_volexgrid(PointCloud::Ptr cloud_src, PointCloud::Ptr &cloud_filter) {
	pcl::VoxelGrid<pcl::PointXYZ> filter;
	filter.setLeafSize(0.003, 0.003, 0.003);
	filter.setInputCloud(cloud_src);
	filter.filter(*cloud_filter);
	cout << "降采样后点云数量：" << cloud_filter->size() << endl;
}

//计算分辨率
void cal_resolution(PointCloud::Ptr cloud) {
	double resolution = 0.0;
	int numberOfPoints = 0;
	int nres;
	std::vector<int> indices(2);
	std::vector<float> squaredDistances(2);
	pcl::search::KdTree<pcl::PointXYZ> tree;

	tree.setInputCloud(cloud);
	for (size_t i = 0; i < cloud->size(); ++i)
	{
		if (!pcl_isfinite((*cloud)[i].x))
			continue;

		// Considering the second neighbor since the first is the point itself.
		nres = tree.nearestKSearch(i, 2, indices, squaredDistances);
		if (nres == 2)
		{
			resolution += sqrt(squaredDistances[1]);
			++numberOfPoints;
		}
	}
	if (numberOfPoints != 0)
		resolution /= numberOfPoints;
	cout << "src cloud resolution:" << resolution << endl;
}
//估计法向量
void est_normals(PointCloud::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals) {
	pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
	norm_est.setNumberOfThreads(4);   //手动设置线程数
	norm_est.setKSearch(10);         //设置k邻域搜索阈值为10个点
	//norm_est.setRadiusSearch(0.001);

	norm_est.setInputCloud(cloud);   //设置输入模型点云
	norm_est.compute(*cloud_normals);//计算点云法线


}


//估计pfh描述子
void est_PFH(PointCloud::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr cloud_normals,
	pcl::PointCloud<pcl::PFHSignature125>::Ptr &cloud_descriptors_pfh)
{
	clock_t start = clock();

	pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_PFH(new pcl::search::KdTree<pcl::PointXYZ>);
	pfh.setInputCloud(cloud);  // 计算keypoints处的特征
	pfh.setInputNormals(cloud_normals);   // cloud的法线
	pfh.setSearchSurface(cloud); // 计算的平面是cloud 而不是keypoints
	pfh.setSearchMethod(kdtree_PFH);
	pfh.setKSearch(10);
//	pfh.setRadiusSearch(0.005);//要大于法线估计的半径	// Search radius, to look for neighbors. Note: the value given here has to be larger than the radius used to estimate the normals.
	pfh.compute(*cloud_descriptors_pfh);
	clock_t end = clock();
	cout << "Time model pfh: " << (double)(end - start) / CLOCKS_PER_SEC << endl;
	cout << "Get model pfh: " << cloud_descriptors_pfh->points.size() << endl;
}

//估计fpfh描述子
void est_FPFH(PointCloud::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr cloud_normals,
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr &cloud_descriptors_fpfh) {

	clock_t start = clock();
	//fpfh估计
	//pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> est_target_fpfh;
	pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> est_fpfh;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_FPFH(new pcl::search::KdTree<pcl::PointXYZ>);

	est_fpfh.setNumberOfThreads(4);//指定4核计算

	est_fpfh.setInputCloud(cloud);
	est_fpfh.setInputNormals(cloud_normals);
	est_fpfh.setSearchMethod(kdtree_FPFH);
	est_fpfh.setKSearch(10);
//	est_fpfh.setRadiusSearch(0.005);
	est_fpfh.compute(*cloud_descriptors_fpfh);
	clock_t end = clock();
	cout << "Time model fpfh: " << (double)(end - start) / CLOCKS_PER_SEC << endl;
	cout << "Get model fpfh: " << cloud_descriptors_fpfh->points.size() << endl;
}


Eigen::Matrix4f pfh_match(PointCloud::Ptr input, pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_input,
	PointCloud::Ptr target, pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_target,
	PointCloud::Ptr &final)
{
	clock_t start = clock();
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::PFHSignature125> sac_ia;
	sac_ia.setInputSource(input);
	sac_ia.setSourceFeatures(cloud_descriptors_pfh_input);
	sac_ia.setInputTarget(target);
	sac_ia.setTargetFeatures(cloud_descriptors_pfh_target);
	//对齐参数设置
	sac_ia.setNumberOfSamples(20);
	sac_ia.setCorrespondenceRandomness(20);//设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
										   //sac_ia.setMaximumIterations(100);
	sac_ia.setEuclideanFitnessEpsilon(0.0005);
	sac_ia.setTransformationEpsilon(1e-10);
	sac_ia.setRANSACIterations(30);
	sac_ia.align(*final);

	cout << "has converged:" << sac_ia.hasConverged() << "score" << sac_ia.getFitnessScore() << endl;	

	clock_t end = clock();
	cout << "pfh Time : " << (double)(end - start) / CLOCKS_PER_SEC << endl;
	return sac_ia.getFinalTransformation();


}

Eigen::Matrix4f fpfh_match(PointCloud::Ptr input, pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_pfh_input,
	PointCloud::Ptr target, pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_pfh_target,
	PointCloud::Ptr &final) {

	clock_t start = clock();
	//对齐  //输入参数 ①源点云+源点特征直方图 ②目标点云+目标点特征直方图
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
	sac_ia.setInputSource(input);
	sac_ia.setSourceFeatures(cloud_descriptors_pfh_input);
	sac_ia.setInputTarget(target);
	sac_ia.setTargetFeatures(cloud_descriptors_pfh_target);


	//对齐参数设置
	sac_ia.setNumberOfSamples(30);
	sac_ia.setCorrespondenceRandomness(30);//设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
										  //sac_ia.setMaximumIterations(100);
	sac_ia.setEuclideanFitnessEpsilon(0.001);
	sac_ia.setTransformationEpsilon(1e-10);
	sac_ia.setRANSACIterations(30);
	sac_ia.align(*final);
	cout << "has converged:" << sac_ia.hasConverged() << "score" << sac_ia.getFitnessScore() << endl;
	clock_t end = clock();
	cout << "fpfh Time : " << (double)(end - start) / CLOCKS_PER_SEC << endl;
	return sac_ia.getFinalTransformation();
}

void match_pfh_sac_icp(PointCloud::Ptr src, pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_src,
	PointCloud::Ptr target, pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_target,
	PointCloud::Ptr &final) {
	clock_t start = clock();
	PointCloud::Ptr temp;
	//SAC配准
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::PFHSignature125> sac_ia;
	sac_ia.setInputSource(src);
	sac_ia.setSourceFeatures(cloud_descriptors_pfh_src);
	sac_ia.setInputTarget(target);
	sac_ia.setTargetFeatures(cloud_descriptors_pfh_target);
	//对齐参数设置
	sac_ia.setNumberOfSamples(60);
	sac_ia.setCorrespondenceRandomness(20);//设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
										   //sac_ia.setMaximumIterations(100);
	sac_ia.setEuclideanFitnessEpsilon(0.0005);
	sac_ia.setTransformationEpsilon(1e-10);
	sac_ia.setRANSACIterations(50);
	sac_ia.align(*final);

	pcl::io::savePLYFile("sac.ply", *final);

	Eigen::Matrix4f sac_trans = sac_ia.getFinalTransformation();
	//icp配准
	PointCloud::Ptr icp_result(new PointCloud);
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(src);
	icp.setInputTarget(target);
	//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance(0.01);
	
	// 最大迭代次数
	icp.setMaximumIterations(50);
	// 两次变化矩阵之间的差值
	icp.setTransformationEpsilon(1e-10);
	// 均方误差
	icp.setEuclideanFitnessEpsilon(0.001);
	icp.align(*final, sac_trans);

	pcl::io::savePLYFile("icp.ply", *final);
	Eigen::Matrix4f icp_trans=icp.getFinalTransformation();
	clock_t end = clock();
	cout << "sac+icp Time : " << (double)(end - start) / CLOCKS_PER_SEC << endl;
	
}
void match_fpfh_sac_icp(PointCloud::Ptr src, pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_pfh_src,
	PointCloud::Ptr target, pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_pfh_target,
	PointCloud::Ptr &final, Eigen::Matrix4f &sac_trans, Eigen::Matrix4f &icp_trans) {
	clock_t start = clock();
	PointCloud::Ptr temp;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_FPFH(new pcl::search::KdTree<pcl::PointXYZ>);

	//SAC配准
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
	sac_ia.setInputSource(src);
	sac_ia.setSourceFeatures(cloud_descriptors_pfh_src);
	sac_ia.setInputTarget(target);
	sac_ia.setTargetFeatures(cloud_descriptors_pfh_target);
	//对齐参数设置
	//sac_ia.setNumberOfSamples(50);
	//sac_ia.setSearchMethodSource(kdtree_FPFH, false);
	sac_ia.setMaxCorrespondenceDistance(0.0005);
	sac_ia.setCorrespondenceRandomness(20);//设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
										   //sac_ia.setMaximumIterations(100);
	sac_ia.setEuclideanFitnessEpsilon(0.0005);
	sac_ia.setTransformationEpsilon(1e-10);
	sac_ia.setRANSACIterations(100);
	sac_ia.align(*final);

	pcl::io::savePLYFile("sac_fpfh.ply", *final);

	sac_trans = sac_ia.getFinalTransformation();
	//icp配准
	PointCloud::Ptr icp_result(new PointCloud);
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(src);
	icp.setInputTarget(target);
	//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance(0.01);

	// 最大迭代次数
	icp.setMaximumIterations(50);
	// 两次变化矩阵之间的差值
	icp.setTransformationEpsilon(1e-10);
	// 均方误差
	icp.setEuclideanFitnessEpsilon(0.001);
	icp.align(*final, sac_trans);

	pcl::io::savePLYFile("icp_fpfh.ply", *final);
	icp_trans = icp.getFinalTransformation();
	clock_t end = clock();
	cout << "sac+icp Time : " << (double)(end - start) / CLOCKS_PER_SEC << endl;

}

void showView(PointCloud::Ptr cloud_1, PointCloud::Ptr cloud_2) {
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("点云显示"));//创建窗口
	int v1 = 0;
	viewer->createViewPort(0, 0, 1, 1, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);	//设置背景色
	

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud_1, 255, 0, 0); // green设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_2, 0, 255, 0); // red设置点云颜色
	viewer->addPointCloud<pcl::PointXYZ>(cloud_1, single_color1, "cloud_0_v1", v1);//
	viewer->addPointCloud<pcl::PointXYZ>(cloud_2, single_color2, "cloud_45_v1", v1);//
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_0_v1", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_45_v1", v1);

	viewer->spin();
}
void showView(PointCloud::Ptr cloud_1, PointCloud::Ptr cloud_2, PointCloud::Ptr cloud_3) {
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("点云显示"));//创建窗口
	int v1 = 0;
	viewer->createViewPort(0, 0, 1, 1, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);	//设置背景色


	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud_1, 255, 0, 0); // green设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_2, 0, 255, 0); // red设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloud_2, 0, 0, 255); // red设置点云颜色
	viewer->addPointCloud<pcl::PointXYZ>(cloud_1, single_color1, "cloud_0_v1", v1);//
	viewer->addPointCloud<pcl::PointXYZ>(cloud_2, single_color2, "cloud_45_v1", v1);//
	viewer->addPointCloud<pcl::PointXYZ>(cloud_3, single_color3, "cloud_90_v1", v1);//
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_0_v1", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_45_v1", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_90_v1", v1);
	viewer->spin();
}
//string file1,string file2, Eigen::Matrix4f
void two_cloud_match(std::string file1, std::string file2, std::string outfile,Eigen::Matrix4f &sac_trans, Eigen::Matrix4f &icp_trans,Eigen::Matrix4f &final_trans) {

	clock_t start = clock();


	//1.读入点云
	PointCloud::Ptr cloud_src_target(new PointCloud);    //model点云  读入
	PointCloud::Ptr cloud_src_src(new PointCloud);    //scene点云
	PointCloud::Ptr cloud_src_refer(new PointCloud);
	//pcl::io::loadPLYFile("dragonStandRight_96.ply", *cloud_src_90);
	//pcl::io::loadPLYFile("dragonStandRight_24.ply", *cloud_src_45);
	//pcl::io::loadPLYFile("dragonStandRight_0.ply", *cloud_src_0);
	pcl::io::loadPLYFile("dragonStandRight_0.ply", *cloud_src_refer);
	pcl::io::loadPLYFile(file2, *cloud_src_src);
	pcl::io::loadPLYFile(file1, *cloud_src_target);
	cout << "/" << endl;
	cout << "原始model点云数量：" << cloud_src_target->size() << endl;
	cout << "原始scene点云数量：" << cloud_src_src->size() << endl;
	cout << "原始scene点云数量：" << cloud_src_refer->size() << endl;

	//2.体素采样滤波
	PointCloud::Ptr cloud_filter_target(new PointCloud);    //model点云  降采样后,一切计算是以这个为基础的
	PointCloud::Ptr cloud_filter_src(new PointCloud);    //scene点云
	PointCloud::Ptr cloud_filter_refer(new PointCloud);    //scene点云
	filter_volexgrid(cloud_src_target, cloud_filter_target);
	filter_volexgrid(cloud_src_src, cloud_filter_src);
	filter_volexgrid(cloud_src_refer, cloud_filter_refer);




	//3.计算点云分辨率
	cal_resolution(cloud_filter_target);
	cal_resolution(cloud_filter_src);
	cal_resolution(cloud_filter_refer);
	cal_resolution(cloud_src_src);

	//4.计算采样后的法线
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_target(new pcl::PointCloud<pcl::Normal>);  // 法向量
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_src(new pcl::PointCloud<pcl::Normal>);  // 法向量
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_refer(new pcl::PointCloud<pcl::Normal>);  // 法向量
	est_normals(cloud_filter_target, cloud_normals_target);
	est_normals(cloud_filter_src, cloud_normals_src);
	est_normals(cloud_filter_refer, cloud_normals_refer);



	//6.计算pfh描述子
	pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_target(new pcl::PointCloud<pcl::PFHSignature125>());  // pfh特征
	pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_src(new pcl::PointCloud<pcl::PFHSignature125>());
	pcl::PointCloud<pcl::PFHSignature125>::Ptr cloud_descriptors_pfh_refer(new pcl::PointCloud<pcl::PFHSignature125>());
	est_PFH(cloud_filter_target, cloud_normals_target, cloud_descriptors_pfh_target);
	est_PFH(cloud_filter_src, cloud_normals_src, cloud_descriptors_pfh_src);
	est_PFH(cloud_filter_refer, cloud_normals_refer, cloud_descriptors_pfh_refer);


	pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_fpfh_target(new pcl::PointCloud<pcl::FPFHSignature33>());  // pfh特征
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_fpfh_src(new pcl::PointCloud<pcl::FPFHSignature33>());
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_descriptors_fpfh_refer(new pcl::PointCloud<pcl::FPFHSignature33>());
	est_FPFH(cloud_filter_target, cloud_normals_target, cloud_descriptors_fpfh_target);
	est_FPFH(cloud_filter_src, cloud_normals_src, cloud_descriptors_fpfh_src);
	est_FPFH(cloud_filter_refer, cloud_normals_refer, cloud_descriptors_fpfh_refer);


	//7.特征点匹配
	PointCloud::Ptr aligned_src2target(new PointCloud);    // 变换之后的点云
	PointCloud::Ptr aligned_src2refer(new PointCloud);    // 变换之后的点云
	PointCloud::Ptr aligned_target2refer(new PointCloud);    // 变换之后的点云
	PointCloud::Ptr aligned_src2target_icp(new PointCloud);    // 变换之后的点云
	PointCloud::Ptr aligned_src2refer_icp(new PointCloud);    // 变换之后的点云
	PointCloud::Ptr aligned_target2refer_icp(new PointCloud);    // 变换之后的点云
	Eigen::Matrix4f sac_trans_target2refer;
	Eigen::Matrix4f sac_trans_src2refer;
	//	sac_trans_45_to_0 = pfh_match(cloud_45, cloud_descriptors_pfh_45, cloud_0, cloud_descriptors_pfh_0, aligned_model_0_45);
	//	sac_trans_90_to_45 = pfh_match(cloud_90, cloud_descriptors_pfh_90, cloud_45, cloud_descriptors_pfh_45, aligned_model_45_90);
	//match_pfh_sac_icp(cloud_45, cloud_descriptors_pfh_45, cloud_0, cloud_descriptors_pfh_0, aligned_model_0_45_icp);

	match_fpfh_sac_icp(cloud_filter_src, cloud_descriptors_fpfh_src, cloud_filter_target, cloud_descriptors_fpfh_target, aligned_src2target_icp,sac_trans,icp_trans);

	pcl::transformPointCloud(*aligned_src2target_icp, *aligned_src2refer_icp, final_trans);

	pcl::io::savePLYFile(outfile, *aligned_src2refer_icp);
	final_trans = icp_trans*final_trans;
	/*
	sac_trans_45_to_0 = fpfh_match(cloud_45, cloud_descriptors_fpfh_45, cloud_0, cloud_descriptors_fpfh_0, aligned_model_0_45);
	sac_trans_90_to_45 = fpfh_match(cloud_90, cloud_descriptors_fpfh_90, cloud_45, cloud_descriptors_fpfh_45, aligned_model_45_90);*/


	//pcl::transformPointCloud(*aligned_model_45_90, *aligned_model_0_90, sac_trans_45_to_0);


	10.显示
	//showView(cloud_filter_target, aligned_src2target_icp);
	//showView(cloud_filter_refer, cloud_filter_target,aligned_src2refer_icp);

//	int v1 = 0;
//	int v2 = 1;
//
//	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("点云显示"));//创建窗口
//
//	viewer->createViewPort(0, 0, 0.5, 1, v1);
//	viewer->createViewPort(0.5, 0, 1, 1, v2);
//	viewer->setBackgroundColor(0, 0, 0, v1);	//设置背景色
//	viewer->setBackgroundColor(0, 0, 0, v2);	//设置背景色
//
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud_filter_target, 255, 0, 0); // green设置点云颜色
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_filter_src, 0, 255, 0); // red设置点云颜色
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloud_filter_refer, 0, 0, 255); // red设置点云颜色
//	viewer->addPointCloud<pcl::PointXYZ>(cloud_filter_target, single_color1, "cloud_0_v1", v1);//
//	viewer->addPointCloud<pcl::PointXYZ>(cloud_filter_src, single_color2, "cloud_45_v1", v1);//
//	viewer->addPointCloud<pcl::PointXYZ>(cloud_filter_refer, single_color3, "cloud_90_v1", v1);//
//	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_0_v1", v1);
//	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_45_v1", v1);
//	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_90_v1", v1);
//
//
//
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color4(cloud_filter_refer, 0, 0, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_0_45_v2(aligned_model_0_45, 0, 255, 0);
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color5(aligned_src2target_icp, 0, 255, 0);
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color6(aligned_src2refer_icp, 255, 0, 0);
//
//	viewer->addPointCloud(cloud_filter_refer, single_color4, "cloud_0_v2", v2);//原始坐标系
//	//	viewer->addPointCloud(aligned_model_0_45, cloud_0_45_v2, "aligend_cloud1_v2", v2);
//	viewer->addPointCloud(aligned_src2target_icp, single_color5, "aligend_cloud2_v2", v2);
//	viewer->addPointCloud(aligned_src2refer_icp, single_color6, "aligend_cloud3_v2", v2);
//	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_0_v2", v2);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "aligend_cloud1_v2", v2);
//	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "aligend_cloud2_v2", v2);
//	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "aligend_cloud3_v2", v2);
//
//	//viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud__filter, normals,1000,0.1);
//	//viewer->addCoordinateSystem(1.0);
//	viewer->spin();
//


	clock_t end = clock();
	cout << "Time : " << (double)(end - start) / CLOCKS_PER_SEC << endl;
}


void generatePLYFile(std::string &file1, std::string &outfile) {
	PointCloud::Ptr cloud_src_target(new PointCloud);    //model点云  读入

	pcl::io::loadPLYFile(file1, *cloud_src_target);
	cout << "/" << endl;
	cout << "原始model点云数量：" << cloud_src_target->size() << endl;


	//2.体素采样滤波
	PointCloud::Ptr cloud_filter_target(new PointCloud);    //model点云  降采样后,一切计算是以这个为基础的

	filter_volexgrid(cloud_src_target, cloud_filter_target);
	pcl::io::savePLYFile( outfile, *cloud_filter_target);
}
void merge_cloud(PointCloud::Ptr &cloud_1, PointCloud::Ptr &cloud_2) {
	*cloud_1 = (*cloud_1) + (*cloud_2);
}

//生成文件名
void generate_filename(int i, std::string &file1, std::string &file2, std::string &outfile, std::string &outfile_src) {
	std::string pre = "dragonStandRight_";
	std::string suff = ".ply";
	//整形转换成字符串
	char str0[4], str1[4];                        // str的长度最少为3，因为10虽然两位，但还有一个结束符
	_itoa_s(i, str0, sizeof(str0), 10);    // int转为字符串
	_itoa_s(i + 24, str1, sizeof(str1), 10);    // int转为字符串
	file1.append(pre);
	file1.append(str0);
	file1.append(suff);
	file2.append(pre);
	file2.append(str1);
	file2.append(suff);
	outfile.append("final_cloud");
	outfile.append(str0);
	outfile.append(".ply");
	outfile_src.append("src_cloud");
	outfile_src.append(str0);
	outfile_src.append(".ply");
}
//生成文件名
void generate_filename(int i, std::string &file1, std::string &file2, std::string &outfile) {
	std::string pre = "dragonStandRight_";
	std::string suff = ".ply";
	//整形转换成字符串
	char str0[4], str1[4];                        // str的长度最少为3，因为10虽然两位，但还有一个结束符
	_itoa_s(i, str0, sizeof(str0), 10);    // int转为字符串
	_itoa_s(i + 24, str1, sizeof(str1), 10);    // int转为字符串
	file1.append(pre);
	file1.append(str0);
	file1.append(suff);
	file2.append(pre);
	file2.append(str1);
	file2.append(suff);
	outfile.append("final_cloud");
	outfile.append(str0);
	outfile.append(".ply");
}


int main(int argc, char** argv) {
	//单位矩阵
	Eigen::Matrix4f final_trans = Eigen::Matrix4f::Identity(4, 4);


	for (int i = 0; i <= 312; i += 24) {
		
		std::string file1, file2,outfile,outfile_src;
		Eigen::Matrix4f sac_trans, icp_trans;

		generate_filename(i, file1, file2, outfile);

		cout << outfile << endl;
		two_cloud_match(file1, file2, outfile, sac_trans, icp_trans,final_trans);


	}
	system("pause");
	return 0;
}
thirty.st
关注
  • 1
    点赞
  • 踩
  • 9
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值