流程，搭建一个龙的demo"/>
三维重建基本流程，搭建一个龙的demo
最近在学习三维重建的一些知识，作为一个小白，从入门到完成一次重建过程是一个需要花一段比较长时间的过程，在网上搜索各种资料时，会因为资料不全，导致无法理清楚逻辑，从而导致入门困难。
我主要是通过书和网上的博客来学习的，如果有时间的话，建议看书，会更加深入的理解整个过程，我这里推荐经典的一本书《点云库PCL从入门到精通》。
这里我将按照我的理解尽可能用简洁的语言来讲清楚三维重建这件事情。
什么是三维重建
你已知一个事物或者一个场景从各个角度得到的三维点云片段，点云与点云之间存在部分重合，你需要做的就是通过某种方式将这些点云拼接在一起，这就是所谓的三维重建。
点云从哪里来
有人会问点云从哪里来，点云的来源有很多种方式，常见的是通过双目视觉，深度相机，结构光或者声纳等硬件设备来获取三维点云，这里不详细讲了
点云重建过程
当你拿到一系列点云时，你需要做以下步骤来进行点云拼接
- 采样处理，由于你拿到的点云是非常庞大的，所以你需要进行采样处理，减少计算量，这里我采用的是下体素采样。
- 法向量估计，两幅点云之所以能较为准确的拼接在一起，是因为两幅点云之间有共同的特征，这里选用的是法线特征，所以你需要估计法线特征
- 计算描述子，这里使用的是fpfh描述子，描述子是用来对匹配两幅点云的，根据法向量才能得到描述子，通过描述子才能知道两幅图的哪些点云是匹配的
- 特征点匹配，特征点匹配分为两步，第一步是粗配，使两个点云基本重合，这里用的方法是采样一致性粗配，然后通过icp方法精配，这样就能得到拼接完成的点云了，但是精度跟你设置的参数有关，主要是setMaxCorrespondenceDistance()方法设置最大匹配的距离，记住粗配时要设得尽量大一点，这样才能搜索到正确的匹配点，精配时需要设置小一点，否则会匹配失效（这是因为ICP只有在两个点云非常接近的时候才能使用，ICP算法没有用到特征点，而是直接根据点云与点云之间的最小距离来匹配点云，属于硬配，不了解ICP算法的同学可以去查一下相关资料）。
- 点云坐标变换，根据第四步你可以得到两点云之间的变换矩阵，然后如果以某一点云作为基准点云，那么你就可以得到其他点云与这个点云之间的变换矩阵，然后通过变换将所有点云旋转平移到统一坐标系下，即可得到重建完的点云图。
以上是点云坐标转换最基本的流程，适合初学者完成一个简单的demo
你需要做的一些前期工作
- c++开发环境，我选用的是VS2015
- 配置PCL点云库
- 点云集链接：
提取码：orjj
将点云集解压后放在c++项目的根目录下，然后就可以运行了。
下面是代码，供大家参考，有任何问题可以在留言区给我留言~
//
///* 目标:对输入的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 floatoperator () (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 而不是keypointspfh.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;
}更多推荐
三维重建基本流程,搭建一个龙的demo
发布评论