init

.gitignore

+build/
+.vscode/
+.VSCodeCounter/
+*.DS_Store
+*.user
+*.autosave
+*.o
+*.doc#
+Library/

CMakeLists.txt

+# 使用CMake Tools插件（可选，如果这个项目去到一个没有这个插件的机器也同样可以生成项目）
+include(CMakeToolsHelpers OPTIONAL)
+ 
+cmake_minimum_required(VERSION 2.8.3)
+project(RegTest)
+find_package(PCL 1.8 REQUIRED)
+ 
+ 
+# 指定构建project所需要的资源
+include_directories(${PCL_INCLUDE_DIRS})#包含头文件的位置
+link_directories(${PCL_LIBRARY_DIRS})#添加链接器的lib库文件路径
+add_definitions(${PCL_DEFINITIONS})
+ 
+#除此之外也可以手动添加特殊路径如：
+#include_directories("G:/Matlab/extern/include")
+#include_directories("C:/Program Files/MobileRobots/Aria/include")
+ 
+add_executable (RegTest RegTest.cpp)
+# add_executable (RegTest src/plyReg.cpp)
+#在生成对应的exe文件之后，需要调用PCL相关函数，因此需要添加相应链接库：
+target_link_libraries(RegTest ${PCL_LIBRARIES})
\ No newline at end of file

RegTest.cpp

+#include <iostream> //标准输入输出头文件
+#include <pcl/console/parse.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_representation.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/conversions.h>
+#include <pcl/filters/uniform_sampling.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/random_sample.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/fpfh.h>
+#include <pcl/registration/correspondence_estimation.h>
+#include <pcl/registration/correspondence_rejection_distance.h>
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <Eigen/Eigen>
+#include <Eigen/Eigenvalues>
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::console;
+using namespace pcl::registration;
+double
+computeCloudResolution(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
+{
+  double res = 0.0;
+  int n_points = 0;
+  int nres;
+  std::vector<int> indices(2);
+  std::vector<float> sqr_distances(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, sqr_distances);
+    if (nres == 2)
+    {
+      res += sqrt(sqr_distances[1]);
+      ++n_points;
+    }
+  }
+  if (n_points != 0)
+  {
+    res /= n_points;
+  }
+  return res;
+}
+
+void estimateFPFH(PointCloud<PointXYZ>::Ptr model,
+                  PointCloud<Normal>::Ptr normals_,
+                  PointCloud<PointXYZ>::Ptr model_keypoints,
+                  PointCloud<FPFHSignature33>::Ptr features_,
+                  std::string name_)
+{
+  static int count_seed = 1;
+  count_seed = count_seed + 5;
+  //---------------1.采样-----------//
+  // 均匀采样：在当前球体所有点中选择距离球体中心最近的点替代所有点
+  // pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
+  // uniform_sampling.setRadiusSearch(2); // 单位m
+  // uniform_sampling.setInputCloud(model);
+  // uniform_sampling.filter(*model_keypoints); // 出错
+
+  // 网格化后的每一个格子称为体素，在体素中包含一些点，然后对这些点取平均或加权平均得到一个点
+  // pcl::VoxelGrid<pcl::PointXYZ> sor; // 创建体素网格采样处理对象
+  // sor.setInputCloud(model);          // 设置输入点云
+  // sor.setLeafSize(2.0f, 2.0f, 2.0f); // 设置体素大小，单位：mm
+  // sor.filter(*model_keypoints);      // 进行下采样
+
+  // 随机下采样
+  pcl::RandomSample<pcl::PointXYZ> rs; // 创建滤波器对象
+  rs.setInputCloud(model);             // 设置待滤波点云
+  rs.setSample(20000);                 // 设置下采样点云的点数
+  rs.setSeed(count_seed);              // 设置随机函数种子点
+  rs.filter(*model_keypoints);
+
+  std::cout << "Model total points: " << model->size() << "; Selected Keypoints: " << model_keypoints->size() << std::endl;
+  pcl::io::savePCDFileBinary<pcl::PointXYZ>("keypoints_tgt.pcd", *model_keypoints);
+
+  //---------------2.1法向量计算-----------//
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr search_method_xyz_(new pcl::search::KdTree<pcl::PointXYZ>);
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
+  norm_est.setInputCloud(model_keypoints);
+  norm_est.setSearchMethod(search_method_xyz_);
+  norm_est.setRadiusSearch(6); // K半径邻域拟合的，所有搜索半径应该大于体素的下采样边长
+  // norm_est.setKSearch(10); //K点数邻域拟合的
+  norm_est.compute(*normals_);
+
+  //---------------2.2法向量储存-----------//
+  // pcl::PointCloud<pcl::PointNormal> sn;
+  // pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*model_keypoints, sn);
+  // pcl::copyPointCloud<pcl::Normal, pcl::PointNormal>(*normals_, sn);
+  // pcl::io::savePCDFileBinary(name_+"normals_tgt.pcd", sn);
+  // std::cout << "Estimated " << normals_->points.size() << " normals for the source datasets." << std::endl;
+
+  //---------------2.3法向量可视化-----------//
+  // pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Normals"));
+  // viewer->setBackgroundColor(0, 0, 0);
+  // //按照z值进行渲染点的颜色
+  // pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(model_keypoints, "x");
+  // // 添加需要显示的点云数据
+  // viewer->addPointCloud<pcl::PointXYZ>(model_keypoints, fildColor, "bunny cloud");
+  // // viewer->addPointCloud<pcl::PointXYZ>(model_keypoints, "bunny cloud");
+  // // 设置点显示大小
+  // viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "bunny cloud");
+  // // Concatenate the XYZ and normal fields* 将点云数据与法向信息拼接
+  // pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>); // pcl::PointNormal, pcl::PointXYZINormal, pcl::PointXYZRGBNormal
+  // pcl::concatenateFields(*model_keypoints, *normals_, *cloud_with_normals);
+  // // 添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，1表示需要显示法向的点云间隔，即每1个点显示一次法向，0.003表示法向长度。
+  // viewer->addPointCloudNormals<pcl::PointNormal>(cloud_with_normals, 1, 10, "normals");
+
+  // while (!viewer->wasStopped()) {
+  //   viewer->spinOnce(100);
+  // }
+
+  //-------------3.特征描述符号---------//
+  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
+  fpfh_est.setInputCloud(model_keypoints);
+  fpfh_est.setInputNormals(normals_);
+  fpfh_est.setSearchMethod(search_method_xyz_);
+  fpfh_est.setRadiusSearch(10);
+  // fpfh_est.setKSearch(20); //需要大于法线估计的邻域
+  fpfh_est.compute(*features_);
+  std::cout << "Estimated " << features_->points.size() << " FPFHSignature33 for the source datasets." << std::endl;
+
+  //-------------3.2特征描述符号存储---------//
+  // pcl::PCLPointCloud2 s, t, out;
+  // pcl::toPCLPointCloud2(*model_keypoints, s);
+  // pcl::toPCLPointCloud2(*features_, t);
+  // pcl::concatenateFields(s, t, out);
+  // pcl::io::savePCDFile(name_+"fpfhs_src.pcd", out);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void findCorrespondences(const PointCloud<FPFHSignature33>::Ptr &fpfhs_src,
+                         const PointCloud<FPFHSignature33>::Ptr &fpfhs_tgt,
+                         Correspondences &all_correspondences)
+{
+  CorrespondenceEstimation<FPFHSignature33, FPFHSignature33> est;
+  est.setInputSource(fpfhs_src);
+  est.setInputTarget(fpfhs_tgt);
+  est.determineReciprocalCorrespondences(all_correspondences);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void rejectBadCorrespondences(const CorrespondencesPtr &all_correspondences,
+                              const PointCloud<PointXYZ>::Ptr &keypoints_src,
+                              const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
+                              pcl::Correspondences &remaining_correspondences)
+{
+  CorrespondenceRejectorDistance rej;
+  rej.setInputSource<PointXYZ>(keypoints_src);
+  rej.setInputTarget<PointXYZ>(keypoints_tgt);
+  rej.setMaximumDistance(10); // 1m
+  rej.setInputCorrespondences(all_correspondences);
+  rej.getCorrespondences(remaining_correspondences);
+}
+
+/*
+点特征直方图（PFH）是一种的姿态不变的局部特征。其计算方式是通过参数化查询点与邻域点之间的空间差异，并形成一个多维直方图对点的k邻域内的几何属性进行描述。
+*/
+void computeTransformation(const PointCloud<PointXYZ>::Ptr &src,
+                           const PointCloud<PointXYZ>::Ptr &tgt,
+                           Eigen::Matrix4f &transform)
+{
+  // Get an uniform grid of keypoints
+  PointCloud<PointXYZ>::Ptr keypoints_src(new PointCloud<PointXYZ>),
+      keypoints_tgt(new PointCloud<PointXYZ>);
+
+  PointCloud<Normal>::Ptr normals_src(new PointCloud<Normal>),
+      normals_tgt(new PointCloud<Normal>);
+
+  PointCloud<FPFHSignature33>::Ptr fpfhs_src(new PointCloud<FPFHSignature33>),
+      fpfhs_tgt(new PointCloud<FPFHSignature33>);
+  estimateFPFH(src, normals_src, keypoints_src, fpfhs_src, "src");
+  estimateFPFH(tgt, normals_tgt, keypoints_tgt, fpfhs_tgt, "tgt");
+  // Find correspondences between keypoints in FPFH space
+  CorrespondencesPtr all_correspondences(new pcl::Correspondences),
+      good_correspondences(new pcl::Correspondences);
+  findCorrespondences(fpfhs_src, fpfhs_tgt, *all_correspondences);
+  std::cout << "all_correspondences size " << all_correspondences->size() << std::endl;
+
+  // Reject correspondences based on their XYZ distance  ???这个距离应该是要基于FPFHSignature33的33维特征计算的
+  rejectBadCorrespondences(all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences);
+
+  std::cout << "good_correspondences size " << good_correspondences->size() << std::endl; // 这里是0，都不满足设定的阈值距；需要先分析满足要求的距离误差为多少
+
+  for (int i = 0; i < good_correspondences->size(); ++i)
+    std::cout << good_correspondences->at(i) << std::endl;
+  // Obtain the best transformation between the two sets of keypoints given the remaining correspondences
+  TransformationEstimationSVD<PointXYZ, PointXYZ, float> trans_est;
+  trans_est.estimateRigidTransformation(*keypoints_src, *keypoints_tgt, *all_correspondences, transform);
+}
+
+int main()
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr model(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::io::loadPCDFile("../Data/03054_right.pcd", *target);
+
+  Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+
+  // 在 X 轴上定义一个 2.5 米的平移.
+  transform.translation() << 2.5, 6.5, 18.5;
+
+  // 和前面一样的旋转; Z 轴上旋转 theta 弧度
+  Eigen::Vector3f r_axis(5, 4, 6);
+  r_axis.normalize();
+  double theta = 30.0;
+  transform.rotate(Eigen::AngleAxisf(theta, r_axis));
+
+  // 打印变换矩阵
+  printf("\nMethod #2: using an Affine3f\n");
+  std::cout << transform.matrix() << std::endl;
+
+  std::cout << "金标准" << std::endl;
+  std::cout << transform.matrix().inverse() << std::endl;
+
+  // 执行变换，并将结果保存在新创建的 model 中
+  pcl::transformPointCloud(*target, *model, transform);
+
+  Eigen::Matrix4f result = Eigen::Matrix4f::Identity();
+  std::clock_t icp_start = clock();
+  computeTransformation(model, target, result);
+  std::clock_t icp_end = clock();
+  std::cout << "ICP" << std::endl;
+  std::cout << "time : " << (icp_end - icp_start) / 1000.0 << std::endl;
+  // result<<
+  // 0.428429, -0.456034,  0.780053,  -12.4695,
+  // 0.895374,  0.330316, -0.298658,  0.483429,
+  // -0.121466,  0.826392,  0.549838,  -15.1326,
+  //         0,         0,         0,         1;
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr registered_model(new pcl::PointCloud<pcl::PointXYZ>());
+  pcl::transformPointCloud(*model, *registered_model, result);
+  std::cout << "配准结果" << std::endl;
+  std::cout << result << std::endl;
+
+  pcl::visualization::PCLVisualizer viewer;
+  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> reg_src_h(registered_model, 0, 255, 0); // 绿色
+  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 255, 0, 0);               // 红色
+
+  viewer.setBackgroundColor(255, 255, 255);
+  viewer.addPointCloud<pcl::PointXYZ>(registered_model, reg_src_h, "source cloud");
+  viewer.addPointCloud<pcl::PointXYZ>(target, tgt_h, "tgt cloud");
+
+  viewer.spin();
+}