PCL_Implicit Shape Model_隐式形状模型 ISM

http://pointclouds.org/documentation/tutorials/implicit_shape_model.php 英文文档

本教程让我们学会implicit shape model算法，通过pcl::ism::ImplicitShapeModel 类来实现，这个算法是hough变换和Bag of Feature方法的结合，目的是实现：拥有一些已知类别的不同对象的训练集点云；该算法计算某个模型，该模型随后将用于预测给定云中不属于训练集的对象中心。

该算法分为两步：一是训练，二是识别点云中不在训练集里的对象。

训练主要包括六步：

1. 关键点检测。相当于训练集点云的简化。在这步中，利用体素华网格方法简化所有点云，留下来的点被作为keypoints。
2. 对每个关键点进行特征估计。使用了FPFH估计，笔记https://blog.csdn.net/yamgyutou/article/details/105407902。
3. 使用k-means对特征进行聚类构造visual（或几何）words字典。获得的聚类代表visual words。集群中的每个特征都是这个visual words的实例（instance）。
4. 计算每个单个的实例到中心的方向，从关键点到给定的点云质心的方向。
5. 对于每个visual words，计算统计权重。
6. 对于每个估计了特征的关键点，计算权重。

当进行完训练并且获得训练模型后，进行对象搜索（或识别），主要分为：

1. 关键点检测。
2. 特征估计。
3. 在字典中搜索每个特征最近的visual word。
4. 对于训练好的每个visual word的实例，添加具有相应方向和投票权的投票。
5. 上一步给了我们一组指向预期中心的方向和每张选票的权重。为了得到对应于中心的单点，需要对这些投票进行分析。为此，算法使用非最大值抑制方法。用户只需要考虑感兴趣对象的半径，其余的将由ISMVoteList::findStrongestPeaks () 方法实现。

训练模型代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/feature.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/impl/fpfh.hpp>
#include <pcl/recognition/implicit_shape_model.h>
#include <pcl/recognition/impl/implicit_shape_model.hpp>

int
main (int argc, char** argv)
{
  if (argc == 0)
  { std::cout << std::endl;
  std::cout << "Usage: " << argv[0] << "class1.pcd class1_label(int) class2.pcd class2_label" << std::endl << std::endl;
   return (-1);
  }
  unsigned int number_of_training_clouds = (argc - 1) / 2;

  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setRadiusSearch (25.0);

  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> training_clouds;
  std::vector<pcl::PointCloud<pcl::Normal>::Ptr> training_normals;
  std::vector<unsigned int> training_classes;

  //加载要训练的点云，并由于算法需要计算其法线。
  //循环结束后，所有点云被计入training_clouds向量， training_normals 存储法线， training_classes存储相应对象的类索引。
  //命令行的输入是一个点云文件一个索引号，argc是参数个数，argv[]是第几个参数。
  for (unsigned int i_cloud = 0; i_cloud < number_of_training_clouds - 1; i_cloud++)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr tr_cloud(new pcl::PointCloud<pcl::PointXYZ> ());
    if ( pcl::io::loadPCDFile <pcl::PointXYZ> (argv[i_cloud * 2 + 1], *tr_cloud) == -1 )
      return (-1);

    pcl::PointCloud<pcl::Normal>::Ptr tr_normals = (new pcl::PointCloud<pcl::Normal>)->makeShared ();
    normal_estimator.setInputCloud (tr_cloud);
    normal_estimator.compute (*tr_normals);

    unsigned int tr_class = static_cast<unsigned int> (strtol (argv[i_cloud * 2 + 2], 0, 10));

    training_clouds.push_back (tr_cloud);
    training_normals.push_back (tr_normals);
    training_classes.push_back (tr_class);
  }

  //创建特征估计器的实例，在将其传递给ISM算法之前，必须将其设置好
  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >::Ptr fpfh
    (new pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >);
  fpfh->setRadiusSearch (30.0);
  pcl::Feature< pcl::PointXYZ, pcl::Histogram<153> >::Ptr feature_estimator(fpfh);
 
  pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal> ism;
  ism.setFeatureEstimator(feature_estimator);
  ism.setTrainingClouds (training_clouds);
  ism.setTrainingNormals (training_normals);
  ism.setTrainingClasses (training_classes);
  ism.setSamplingSize (2.0f);//提供了用于点云简化的采样大小值

  pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal>::ISMModelPtr model = boost::shared_ptr<pcl::features::ISMModel>
    (new pcl::features::ISMModel);
  ism.trainISM (model);//进行训练过程

  std::string file ("trained_ism_model.txt");
  model->saveModelToFile (file);//保存获得的训练模型，方便再次使用

  std::cout << "trained_ism_model.txt is the output of training stage. You can use the trained_ism_model.txt in the classification stage" << std::endl << std::endl;

  return (0);
}

之后在命令行输入：因为程序运行十分缓慢，我就只训练了两个点云。

>implicit_shape_model_training.exe ism_train_cat.pcd 0 ism_train_wolf.pcd 1

 结果：生成训练模型的.txt文件。

 

分类的代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/feature.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/impl/fpfh.hpp>
#include <pcl/recognition/implicit_shape_model.h>
#include <pcl/recognition/impl/implicit_shape_model.hpp>

int
main (int argc, char** argv)
{
    //如果没有参数，输出消息
   if (argc == 0)
  { std::cout << std::endl;
  std::cout << "Usage: " << argv[0] << " test_scene.pcd class1_label(int)" << std::endl << std::endl;
  std::cout << "Where the parameter class1_label is the object you want to be segmented and recognized" << std::endl << std::endl;
   return (-1);
  }

   //创建法线估计实例，创建特征估计器实例
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setRadiusSearch (25.0);
  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >::Ptr fpfh
    (new pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >);
  fpfh->setRadiusSearch (30.0);
  pcl::Feature< pcl::PointXYZ, pcl::Histogram<153> >::Ptr feature_estimator(fpfh);

  pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal> ism;
  ism.setFeatureEstimator(feature_estimator);
  ism.setSamplingSize (2.0f);

  pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal>::ISMModelPtr model = boost::shared_ptr<pcl::features::ISMModel>
    (new pcl::features::ISMModel);
  std::string file ("trained_ism_model.txt");

  //如果直接traning 写在一起，就直接从这里开始，加载训练模型就行
  model->loadModelFromfile (file);
  unsigned int testing_class = static_cast<unsigned int> (strtol (argv[2], 0, 10));//要匹配的类别的索引号
  pcl::PointCloud<pcl::PointXYZ>::Ptr testing_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  if ( pcl::io::loadPCDFile <pcl::PointXYZ> (argv[1], *testing_cloud) == -1 )
    return (-1);

  //计算需要测试的点云的法线
  pcl::PointCloud<pcl::Normal>::Ptr testing_normals = (new pcl::PointCloud<pcl::Normal>)->makeShared ();
  normal_estimator.setInputCloud (testing_cloud);
  normal_estimator.compute (*testing_normals);

  //这里执行分类过程，pcl::ism::ISMVoteList是一个单独的类，目的是帮助你分析投票。
  boost::shared_ptr<pcl::features::ISMVoteList<pcl::PointXYZ> > vote_list = ism.findObjects (
    model,
    testing_cloud,
    testing_normals,
    testing_class);

  double radius = model->sigmas_[testing_class] * 10.0;
  double sigma = model->sigmas_[testing_class];
  std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > strongest_peaks;
  vote_list->findStrongestPeaks (strongest_peaks, testing_class, radius, sigma);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
  colored_cloud->height = 0;
  colored_cloud->width = 1;

  pcl::PointXYZRGB point;
  point.r = 200;
  point.g = 200;
  point.b = 200;
  for (size_t i_point = 0; i_point < testing_cloud->points.size (); i_point++)
  {
	  //创建一个point点云，将testing_cloud的每个点传给point，再通过push_back函数把这个点加入到colored_cloud
    point.x = testing_cloud->points[i_point].x;
    point.y = testing_cloud->points[i_point].y;
    point.z = testing_cloud->points[i_point].z;
    colored_cloud->points.push_back (point);
  }
  colored_cloud->height += testing_cloud->points.size (); //colored_cloud的大小=testing_cloud+colored_cloud

  point.r = 255;
  point.g = 0;
  point.b = 0;
  for (size_t i_vote = 0; i_vote < strongest_peaks.size (); i_vote++)
  {
    point.x = strongest_peaks[i_vote].x;
    point.y = strongest_peaks[i_vote].y;
    point.z = strongest_peaks[i_vote].z;
    colored_cloud->points.push_back (point);//将strongest peaks加入colored_cloud中，红色显示
  }
  colored_cloud->height += strongest_peaks.size ();

  //可视化
  pcl::visualization::PCLVisualizer viewer ("点云库PCL学习教程第二版-隐式形状模型");
  viewer.setBackgroundColor(1,1,1);
  //30,200,30显示原测试数据
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> colorh(testing_cloud,30,200,30);
  viewer.addPointCloud(testing_cloud,colorh,"test_data");
  viewer.addPointCloud (colored_cloud,"centors");
  viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,10,"centors");
  viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"test_data");
  while (!viewer.wasStopped ())
  {
	  viewer.spin();
  }

  return (0);
}

结果：红色表示感兴趣类别对应的预测中心。 

implicit_shape_model_classification.exe ism_test_all.pcd 0

 绿色是要测试点云，灰色是colored_cloud，在猫中可以看到有一个红色质心。

 

---

但是这个程序貌似有bug ？ 

>implicit_shape_model_classification.exe ism_test_wolf.pcd 1
或者
>implicit_shape_model_classification.exe ism_test_all.pcd 1
或者
>implicit_shape_model_classification.exe ism_train_wolf.pcd 1
中的任意一个都会显示如下的错误：

 好像是超出了向量范围之类的。

 

---

4.23更新

我又试着跑了一下，用了三个测试集。

implicit_shape_model_training.exe ism_train_cat.pcd 0 ism_train_horse.pcd 1 ism_train_lioness.pcd 2

implicit_shape_model_classification.exe ism_train_horse.pcd 1