3D object recognition based on related groups

This time we will explain how to use the Pcl_recognition module for 3D object recognition. In particular, it explains how to use the correlation group algorithm in order to cluster those from the 3D descriptor algorithm to match the current scene with the model of the relevant point-to-point matching. (Long hard sentence). For each cluster, a possible model instance in the scene is depicted, and the correlation group algorithm outputs a transformation matrix that identifies the 6DOF pose estimation.

Code

#include <pcl/io/pcd_io.h> #include <pcl/point_cloud.h> #include <pcl/correspondence.h> #include <pcl/features/normal_3d_omp.h> #include <pcl/features/shot_omp.h> #include <pcl/features/board.h > #include <pcl/filters/uniform_sampling.h> #include <pcl/recognition/cg/hough_3d.h> #include &LT;PCL /recognition/cg/geometric_consistency.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/ kdtree/kdtree_flann.h> #include <pcl/kdtree/impl/kdtree_flann.hpp> #include <pcl/common/transforms.h
> #include <pcl/console/parse.h> typedef PCL::P Ointxyzrgba Pointtype;
typedef pcl::normal NORMALTYPE;
typedef pcl::referenceframe RFTYPE;

typedef pcl::shot352 DESCRIPTORTYPE;
Std::string model_filename_;

Std::string scene_filename_;
Algorithm params bool Show_keypoints_ (FALSE);
BOOL Show_correspondences_ (FALSE);
BOOL Use_cloud_resolution_ (FALSE);
BOOL Use_hough_ (TRUE);
Float Model_ss_ (0.01f); Float Scene_ss_ (0.03f);
Float Rf_rad_ (0.015f);
Float Descr_rad_ (0.02f);
Float Cg_size_ (0.01f);

Float Cg_thresh_ (5.0f);
  void ShowHelp (char *filename) {std::cout << Std::endl; Std::cout << "***************************************************************************" << std::
  Endl
  Std::cout << "* *" << Std::endl;
  Std::cout << "* Correspondence Grouping tutorial-usage Guide *" << Std::endl;
  Std::cout << "* *" << Std::endl; Std::cout << "***************************************************************************" << std::
  Endl << Std::endl; Std::cout << "Usage:" << filename << "model_filename.pcd scene_filename.pcd [Options]" << std::e
  NDL << Std::endl;
  Std::cout << "Options:" << Std::endl; Std::cout &Lt;< "-h:show this Help." << Std::endl;
  Std::cout << "-k:show used keypoints." << Std::endl;
  Std::cout << "-c:show used correspondences." << Std::endl;
  Std::cout << "-r:compute the model cloud resolution and multiply" << std::endl;
  Std::cout << "Each radius given by that value." << Std::endl; Std::cout << "--algorithm (hough|
  GC): Clustering algorithm Used (default Hough). "<< Std::endl;
  Std::cout << "--MODEL_SS Val:model Uniform sampling radius (default 0.01)" << Std::endl;
  Std::cout << "--SCENE_SS val:scene Uniform sampling radius (default 0.03)" << Std::endl;
  Std::cout << "--rf_rad val:reference frame radius (default 0.015)" << Std::endl;      Std::cout << "--descr_rad val:  Descriptor radius (default 0.02) "<< Std::endl;
  Std::cout << "--cg_size val:cluster size (default 0.01)" << Std::endl;
Std::cout << "--cg_thresh val:clustering threshold (default 5)" << Std::endl << Std::endl;
    } void ParseCommandLine (int argc, char *argv[]) {//show help if (Pcl::console::find_switch (argc, argv, "-H")) {
    ShowHelp (Argv[0]);
  Exit (0);
  }//model & Scene filenames std::vector<int> filenames;
  Filenames = Pcl::console::p arse_file_extension_argument (argc, argv, ". PCD");
    if (filenames.size ()! = 2) {std::cout << "filenames missing.\n";
    ShowHelp (Argv[0]);
  Exit (-1);
  } model_filename_ = argv[filenames[0]];

  Scene_filename_ = argv[filenames[1]];
  Program Behavior if (Pcl::console::find_switch (argc, argv, "-K")) {Show_keypoints_ = true;
  } if (Pcl::console::find_switch (argc, argv, "-C")) {Show_correspondences_ = true;} if (Pcl::console::find_switch (argc, argv, "-r")) {Use_cloud_resolution_ = true;
  } std::string Used_algorithm; if (pcl::console::p arse_argument (argc, argv, "--algorithm", used_algorithm)! =-1) {if (Used_algorithm.compare ("H
    Ough ") = = 0) {Use_hough_ = true;
    }else if (Used_algorithm.compare ("GC") = = 0) {Use_hough_ = false;
      } else {std::cout << "wrong algorithm name.\n";
      ShowHelp (Argv[0]);
    Exit (-1);
  }}//general Parameters Pcl::console::p arse_argument (argc, argv, "--model_ss", Model_ss_);
  Pcl::console::p arse_argument (argc, argv, "--scene_ss", Scene_ss_);
  Pcl::console::p arse_argument (argc, argv, "--rf_rad", Rf_rad_);
  Pcl::console::p arse_argument (argc, argv, "--descr_rad", Descr_rad_);
  Pcl::console::p arse_argument (argc, argv, "--cg_size", cg_size_);
Pcl::console::p arse_argument (argc, argv, "--cg_thresh", cg_thresh_); } double computecloudresolution (const PCL::P Ointcloud<pointtype>::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<pointtype> 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 (