PCL实战记录（二） 三维识别与乱序抓取

PCL 实战记录 （二）

1、三维点云预处理

为了较好的完成三维识别与乱序抓取，点云输入前需要进行一定的预处理，主要包括：
A、点云采样；
B、点云滤波；
C、点云分割；
以上内容参见上篇：PCL 实战记录 （一）.

2、计算法线

计算法线 normalestimationomp估计局部表面特性在每个三个特点，分别表面的法向量和曲率，平行，使用OpenMP标准。
初始化调度程序并设置要使用的线程数（默认为0）。
平面的法线是垂直于它的单位向量。在点云的表面的法线被定义为垂直于与点云表面相切的平面的向量。表面法线也可以计算点云中一点的法线，被认为是一种十分重要的性质。常常在被使用在很多计算机视觉的应用里面，比如可以用来推出光源的位置，通过阴影与其他视觉影响，表面法线的问题可以近似化解为切面的问题，这个切面的问题又会变成最小二乘法拟合平面的问题

解决表面法线估计的问题可以最终化简为对一个协方差矩阵的特征向量和特征值的分析(或者也叫PCA-Principal Component Analysis 主成分分析)，这个协方差矩阵是由查询点的最近邻产生的。对于每个点Pi,我们假设协方差矩阵C如下:

pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
  norm_est.setKSearch(10);       //设置K邻域搜索阀值为10个点
  norm_est.setInputCloud(model);  //设置输入点云
  norm_est.compute(*model_normals);   //计算点云法线 

  norm_est.setInputCloud(scene);
  norm_est.compute(*scene_normals);

3、计算关键点

均匀采样点云并提取关键点

pcl::PointCloud<int> sampled_indices;
  pcl::UniformSampling<PointType> uniform_sampling;
  uniform_sampling.setInputCloud(model);  //输入点云
  uniform_sampling.setRadiusSearch(model_ss_);   //设置半径 model_ss_初值是0.01可以通过agv修改
  uniform_sampling.compute (sampled_indices);
	pcl::copyPointCloud (*model, sampled_indices.points, *model_keypoints);
	std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << 			model_keypoints->size () << std::endl;

 uniform_sampling.setInputCloud (scene);
 uniform_sampling.setRadiusSearch (scene_ss_);
 uniform_sampling.compute (sampled_indices);
 pcl::copyPointCloud (*scene, sampled_indices.points, *scene_keypoints);
 std::cout << "Scene total points: " << scene->size() << "; Selected Keypoints: " << scene_keypoints->size() << std::endl;
  //均匀采样点云并提取关键点      体素下采样，重心代替

4、计算SHOT描述子

为关键点计算描述子：包括关键点、法线、点云。
除此之外，还包括以下多种特征描述子：

pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
  descr_est.setRadiusSearch(descr_rad_);  //设置搜索半径

  descr_est.setInputCloud(model_keypoints);  //输入模型的关键点
  descr_est.setInputNormals(model_normals);  //输入模型的法线
  descr_est.setSearchSurface(model);         //输入的点云
  descr_est.compute(*model_descriptors);     //计算描述子

  descr_est.setInputCloud(scene_keypoints);  //同理
  descr_est.setInputNormals(scene_normals);
  descr_est.setSearchSurface(scene);
  descr_est.compute(*scene_descriptors);

5、计算配对点对

使用Kdtree找出 Model-Scene 匹配点

//  使用Kdtree找出 Model-Scene 匹配点
  pcl::CorrespondencesPtr model_scene_corrs(new pcl::Correspondences());

  pcl::KdTreeFLANN<DescriptorType> match_search;   //设置配准的方法
  match_search.setInputCloud(model_descriptors);  //输入模板点云的描述子

  //每一个场景的关键点描述子都要找到模板中匹配的关键点描述子并将其添加到对应的匹配向量中。
  for (size_t i = 0; i < scene_descriptors->size(); ++i)
  {
      std::vector<int> neigh_indices(1);   //设置最近邻点的索引
      std::vector<float> neigh_sqr_dists(1); //申明最近邻平方距离值
      if (!pcl_isfinite(scene_descriptors->at(i).descriptor[0])) //忽略 NaNs点
      {
          continue;
      }
      int found_neighs = match_search.nearestKSearch(scene_descriptors->at(i), 1, neigh_indices, neigh_sqr_dists);
      //scene_descriptors->at (i)是给定点云 1是临近点个数 ，neigh_indices临近点的索引  neigh_sqr_dists是与临近点的索引

      if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // 仅当描述子与临近点的平方距离小于0.25（描述子与临近的距离在一般在0到1之间）才添加匹配
      {
          //neigh_indices[0]给定点，  i  是配准数  neigh_sqr_dists[0]与临近点的平方距离
          pcl::Correspondence corr(neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
          model_scene_corrs->push_back(corr);   //把配准的点存储在容器中
      }
  }
  std::cout << "Correspondences found: " << model_scene_corrs->size() << std::endl;

6、寻找匹配点

使用两种方式进行寻找：
A、Hough霍夫投票
B、GCG几何一致性

//  实际的配准方法的实现
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
  std::vector<pcl::Correspondences> clustered_corrs;

  //  使用 Hough3D算法寻找匹配点
  if (use_hough_)
  {
      //
      //  Compute (Keypoints) Reference Frames only for Hough
      //计算参考帧的Hough（也就是关键点）
      cout << "使用3d hough 匹配方法" << endl;
      pcl::PointCloud<RFType>::Ptr model_rf(new pcl::PointCloud<RFType>());
      pcl::PointCloud<RFType>::Ptr scene_rf(new pcl::PointCloud<RFType>());
      //特征估计的方法（点云，法线，参考帧）
      pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
      rf_est.setFindHoles(true);
      rf_est.setRadiusSearch(rf_rad_);   //设置搜索半径

      rf_est.setInputCloud(model_keypoints);  //模型关键点
      rf_est.setInputNormals(model_normals); //模型法线
      rf_est.setSearchSurface(model);    //模型
      rf_est.compute(*model_rf);      //模型的参考帧   

      rf_est.setInputCloud(scene_keypoints);  //同理
      rf_est.setInputNormals(scene_normals);
      rf_est.setSearchSurface(scene);
      rf_est.compute(*scene_rf);

      //  Clustering聚类的方法

      //对输入点与的聚类，以区分不同的实例的场景中的模型 
      pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
      clusterer.setHoughBinSize(cg_size_);//霍夫空间设置每个bin的大小
      clusterer.setHoughThreshold(cg_thresh_);//阀值
      clusterer.setUseInterpolation(true);
      clusterer.setUseDistanceWeight(false);

      clusterer.setInputCloud(model_keypoints);
      clusterer.setInputRf(model_rf);   //设置输入的参考帧
      clusterer.setSceneCloud(scene_keypoints);
      clusterer.setSceneRf(scene_rf);
      clusterer.setModelSceneCorrespondences(model_scene_corrs);//model_scene_corrs存储配准的点

      //clusterer.cluster (clustered_corrs);辨认出聚类的对象
      clusterer.recognize(rototranslations, clustered_corrs);
  }
  else // Using GeometricConsistency  或者使用几何一致性性质
  {
      cout << "使用几何一致性方法" << endl;
      pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
      gc_clusterer.setGCSize(cg_size_);   //设置几何一致性的大小
      gc_clusterer.setGCThreshold(cg_thresh_); //阀值

      gc_clusterer.setInputCloud(model_keypoints);
      gc_clusterer.setSceneCloud(scene_keypoints);
      gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);

      //gc_clusterer.cluster (cl