Exemple #1
0
TEST (PCL, CVFHEstimationMilk)
{

  // Estimate normals first
  NormalEstimation<PointXYZ, Normal> n;
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  n.setInputCloud (cloud_milk);
  n.setSearchMethod (tree);
  n.setRadiusSearch (leaf_size_ * 4); //2cm to estimate normals
  n.compute (*normals);

  CVFHEstimation<PointXYZ, Normal, VFHSignature308> cvfh;
  cvfh.setInputCloud (cloud_milk);
  cvfh.setInputNormals (normals);
  cvfh.setSearchMethod (tree_milk);
  cvfh.setClusterTolerance (leaf_size_ * 3);
  cvfh.setEPSAngleThreshold (0.13f);
  cvfh.setCurvatureThreshold (0.025f);
  cvfh.setNormalizeBins (false);
  cvfh.setRadiusNormals (leaf_size_ * 4);

  // Object
  PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ());

  // estimate
  cvfh.compute (*vfhs);
  EXPECT_EQ (static_cast<int>(vfhs->points.size ()), 2);
}
/* ---[ */
int
  main (int argc, char** argv)
{
  if (argc < 2)
  {
    std::cerr << "No test file given. Please download `sac_plane_test.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  // Load a standard PCD file from disk
  sensor_msgs::PointCloud2 cloud_blob;
  if (loadPCDFile (argv[1], cloud_blob) < 0)
  {
    std::cerr << "Failed to read test file. Please download `sac_plane_test.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }
  fromROSMsg (cloud_blob, *cloud_);

  indices_.resize (cloud_->points.size ());
  for (size_t i = 0; i < indices_.size (); ++i) { indices_[i] = int (i); }

  // Estimate surface normals
  NormalEstimation<PointXYZ, Normal> n;
  search::Search<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ>);
  tree->setInputCloud (cloud_);
  n.setInputCloud (cloud_);
  boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices_));
  n.setIndices (indicesptr);
  n.setSearchMethod (tree);
  n.setRadiusSearch (0.02);    // Use 2cm radius to estimate the normals
  n.compute (*normals_);

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
Exemple #3
0
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         int k, double radius)
{
  // Convert data to PointCloud<T>
  PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
  fromROSMsg (*input, *xyz);

  // Estimate
  TicToc tt;
  tt.tic ();
  
  print_highlight (stderr, "Computing ");

  NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (xyz);
//  ne.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
  ne.setKSearch (k);
  ne.setRadiusSearch (radius);
  
  PointCloud<Normal> normals;
  ne.compute (normals);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", normals.width * normals.height); print_info (" points]\n");

  // Convert data back
  sensor_msgs::PointCloud2 output_normals;
  toROSMsg (normals, output_normals);
  concatenateFields (*input, output_normals, output);
}
Exemple #4
0
TEST (PCL, SHOTGlobalReferenceFrame)
{
  // Estimate normals first
  double mr = 0.002;
  NormalEstimation<PointXYZ, Normal> n;
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  // set parameters
  n.setInputCloud (cloud.makeShared ());
  boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
  n.setIndices (indicesptr);
  n.setSearchMethod (tree);
  n.setRadiusSearch (20 * mr);
  n.compute (*normals);

  EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
  EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
  EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);

  EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);

  boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
  for (size_t i = 0; i < cloud.size (); i+=3)
    test_indices->push_back (static_cast<int> (i));

  testGSHOTGlobalReferenceFrame<GSHOTEstimation<PointXYZ, Normal, SHOT352>, PointXYZ, Normal, SHOT352> (cloud.makeShared (), normals, test_indices);
}
    void
    estimateNormals (const typename PointCloud<PointT>::ConstPtr &input, PointCloud<Normal> &normals)
    {
      if (input->isOrganized ())
      {
        IntegralImageNormalEstimation<PointT, Normal> ne;
        // Set the parameters for normal estimation
        ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
        ne.setMaxDepthChangeFactor (0.02f);
        ne.setNormalSmoothingSize (20.0f);
        // Estimate normals in the cloud
        ne.setInputCloud (input);
        ne.compute (normals);

        // Save the distance map for the plane comparator
        float *map=ne.getDistanceMap ();// This will be deallocated with the IntegralImageNormalEstimation object...
        distance_map_.assign(map, map+input->size() ); //...so we must copy the data out
        plane_comparator_->setDistanceMap(distance_map_.data());
      }
      else
      {
        NormalEstimation<PointT, Normal> ne;
        ne.setInputCloud (input);
        ne.setRadiusSearch (0.02f);
        ne.setSearchMethod (search_);
        ne.compute (normals);
      }
    }
Exemple #6
0
void
computeFeatureViaNormals (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
                          int argc, char** argv, bool set_search_flag = true)
{
    int n_k = default_n_k;
    int f_k = default_f_k;
    double n_radius = default_n_radius;
    double f_radius = default_f_radius;
    parse_argument (argc, argv, "-n_k", n_k);
    parse_argument (argc, argv, "-n_radius", n_radius);
    parse_argument (argc, argv, "-f_k", f_k);
    parse_argument (argc, argv, "-f_radius", f_radius);

    // Convert data to PointCloud<PointIn>
    typename PointCloud<PointIn>::Ptr xyz (new PointCloud<PointIn>);
    fromPCLPointCloud2 (*input, *xyz);

    // Estimate
    TicToc tt;
    tt.tic ();

    print_highlight (stderr, "Computing ");

    NormalEstimation<PointIn, NormalT> ne;
    ne.setInputCloud (xyz);
    ne.setSearchMethod (typename pcl::search::KdTree<PointIn>::Ptr (new pcl::search::KdTree<PointIn>));
    ne.setKSearch (n_k);
    ne.setRadiusSearch (n_radius);

    typename PointCloud<NormalT>::Ptr normals = typename PointCloud<NormalT>::Ptr (new PointCloud<NormalT>);
    ne.compute (*normals);

    FeatureAlgorithm feature_est;
    feature_est.setInputCloud (xyz);
    feature_est.setInputNormals (normals);

    feature_est.setSearchMethod (typename pcl::search::KdTree<PointIn>::Ptr (new pcl::search::KdTree<PointIn>));

    PointCloud<PointOut> output_features;

    if (set_search_flag) {
        feature_est.setKSearch (f_k);
        feature_est.setRadiusSearch (f_radius);
    }

    feature_est.compute (output_features);

    print_info ("[done, ");
    print_value ("%g", tt.toc ());
    print_info (" ms : ");
    print_value ("%d", output.width * output.height);
    print_info (" points]\n");

    // Convert data back
    toPCLPointCloud2 (output_features, output);
}
Exemple #7
0
    // Subsample cloud for faster matching and processing, while filling in normals.
    void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const
    {
      PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced;
      PointCloud<Normal> normals;
      PointCloud<PointXYZRGBNormal> cloud_normals;
      
      std::vector<int> indices;
      
      // Filter out nans.
      removeNaNFromPointCloud(input, cloud_nan_filtered, indices);
      indices.clear();
      
      // Filter out everything outside a [200x200x200] box.
      Eigen::Vector4f min_pt(-100, -100, -100, -100);
      Eigen::Vector4f max_pt(100, 100, 100, 100);
      getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices);
      
      ExtractIndices<PointXYZRGB> boxfilter;
      boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered));
      boxfilter.setIndices (boost::make_shared<vector<int> > (indices));
      boxfilter.filter(cloud_box_filtered);
      
      // Reduce pointcloud
      VoxelGrid<PointXYZRGB> voxelfilter;
      voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered));
      voxelfilter.setLeafSize (0.05, 0.05, 0.05);
      //      voxelfilter.setLeafSize (0.1, 0.1, 0.1);
      voxelfilter.filter (cloud_voxel_reduced);
      
      // Compute normals
      NormalEstimation<PointXYZRGB, Normal> normalest;
      normalest.setViewPoint(0, 0, 0);
      normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ());
      //normalest.setKSearch (10);
      normalest.setRadiusSearch (0.25);
      //      normalest.setRadiusSearch (0.4);
      normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced));
      normalest.compute(normals);
      
      pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals);

      // Filter based on curvature
      PassThrough<PointXYZRGBNormal> normalfilter;
      normalfilter.setFilterFieldName("curvature");
      //      normalfilter.setFilterLimits(0.0, 0.2);
      normalfilter.setFilterLimits(0.0, 0.2);
      normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals));
      normalfilter.filter(output);
    }
    void create_normals (typename PointCloud<PointT>::Ptr cloud,
            typename PointCloud<NormalT>::Ptr normals,
            float normal_radius=0.03)
    {
        NormalEstimation<PointT, NormalT> nest;

        cout << "[PFHTransformationStrategy::create_normals] Input cloud "
            << cloud->points.size() << " points" << endl;

        nest.setInputCloud(cloud);
        nest.setSearchMethod (typename search::KdTree<PointT>::Ptr
                 (new search::KdTree<PointT>));
        nest.setRadiusSearch(normal_radius);
        nest.compute(*normals);
    };
Exemple #9
0
TEST (PCL, GSHOTRadius)
{
  float radius = radius_local_shot / 4.0f;
  
  // Estimate normals first
  double mr = 0.002;
  NormalEstimation<PointXYZ, Normal> n;
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  // set parameters
  n.setInputCloud (cloud.makeShared ());
  boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
  n.setIndices (indicesptr);
  n.setSearchMethod (tree);
  n.setRadiusSearch (20 * mr);
  n.compute (*normals);

  // Objects
  PointCloud<SHOT352>::Ptr gshots352 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ());
  
  // SHOT352 (local)
  SHOTEstimation<PointXYZ, Normal, SHOT352> shot352;
  shot352.setInputNormals (normals);
  shot352.setRadiusSearch (radius);
  shot352.setInputCloud (cloud_for_lrf.makeShared ());
  boost::shared_ptr<vector<int> > indices_local_shot_ptr (new vector<int> (indices_local_shot));
  shot352.setIndices (indices_local_shot_ptr);
  shot352.setSearchSurface (cloud.makeShared());
  shot352.compute (*shots352);

  // SHOT352 (global)
  GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot352;
  gshot352.setInputNormals (normals);
  // set parameters
  gshot352.setInputCloud (cloud.makeShared ());
  gshot352.setIndices (indicesptr);
  gshot352.setSearchMethod (tree);
  gshot352.setRadiusSearch (radius);
  EXPECT_EQ (gshot352.getRadiusSearch (), shot352.getRadiusSearch ());
  // estimate
  gshot352.compute (*gshots352);

  checkDescNear (*gshots352, *shots352, 1E-7);
}
Exemple #10
0
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         int k, double radius)
{
  // Convert data to PointCloud<T>
  PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
  fromROSMsg (*input, *xyz);

  TicToc tt;
  tt.tic ();
 
  PointCloud<Normal> normals;

  // Try our luck with organized integral image based normal estimation
  if (xyz->isOrganized ())
  {
    IntegralImageNormalEstimation<PointXYZ, Normal> ne;
    ne.setInputCloud (xyz);
    ne.setNormalEstimationMethod (IntegralImageNormalEstimation<PointXYZ, Normal>::COVARIANCE_MATRIX);
    ne.setNormalSmoothingSize (float (radius));
    ne.setDepthDependentSmoothing (true);
    ne.compute (normals);
  }
  else
  {
    NormalEstimation<PointXYZ, Normal> ne;
    ne.setInputCloud (xyz);
    ne.setSearchMethod (search::KdTree<PointXYZ>::Ptr (new search::KdTree<PointXYZ>));
    ne.setKSearch (k);
    ne.setRadiusSearch (radius);
    ne.compute (normals);
  }

  print_highlight ("Computed normals in "); print_value ("%g", tt.toc ()); print_info (" ms for "); print_value ("%d", normals.width * normals.height); print_info (" points.\n");

  // Convert data back
  sensor_msgs::PointCloud2 output_normals;
  toROSMsg (normals, output_normals);
  concatenateFields (*input, output_normals, output);
}
Exemple #11
0
/* ---[ */
int
main (int argc, char** argv)
{
  // Load two standard PCD files from disk
  if (argc < 3)
  {
    std::cerr << "No test files given. Please download `sac_plane_test.pcd` and 'cturtle.pcd' and pass them path to the test." << std::endl;
    return (-1);
  }

  // Load in the point clouds
  io::loadPCDFile (argv[1], *cloud_walls);
  io::loadPCDFile (argv[2], *cloud_turtle);



  // Compute the normals for each cloud, and then clean them up of any NaN values
  NormalEstimation<PointXYZ,PointNormal> ne;
  ne.setInputCloud (cloud_walls);
  ne.setRadiusSearch (0.05);
  ne.compute (*cloud_walls_normals);
  copyPointCloud (*cloud_walls, *cloud_walls_normals);

  std::vector<int> aux_indices;
  removeNaNFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
  removeNaNNormalsFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);

  ne = NormalEstimation<PointXYZ, PointNormal> ();
  ne.setInputCloud (cloud_turtle);
  ne.setKSearch (5);
  ne.compute (*cloud_turtle_normals);
  copyPointCloud (*cloud_turtle, *cloud_turtle_normals);
  removeNaNFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
  removeNaNNormalsFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
/*! @brief runs the whole processing pipeline for FPFH features
 *
 * @note At the moment the evaluation results will be printed to console.
 *
 * @param[in] in the labeled input point cloud
 * @param[out] ref_out the reference point cloud after the preprocessing steps
 * @param[out] fpfh_out the labeled point cloud after the classifing process
 */
void processFPFH(const PointCloud<PointXYZRGB>::Ptr in,
                 PointCloud<PointXYZRGB>::Ptr ref_out,
                 PointCloud<PointXYZRGB>::Ptr fpfh_out)
{
  PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
  PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>());

  // Passthrough filtering (needs to be done to remove NaNs)
  cout << "FPFH: Pass (with " << in->points.size() << " points)" << endl;
  PassThrough<PointXYZRGB> pass;
  pass.setInputCloud(in);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(0.0f, pass_depth_);
  pass.filter(*ref_out);

  // Optional voxelgrid filtering
  if (fpfh_vox_enable_)
  {
    cout << "FPFH: Voxel (with " << ref_out->points.size() << " points)" << endl;
    VoxelGrid<PointXYZRGB> vox;
    vox.setInputCloud(ref_out);
    vox.setLeafSize(fpfh_vox_, fpfh_vox_, fpfh_vox_);
    vox.filter(*ref_out);
  }

  #ifdef PCL_VERSION_COMPARE //fuerte
    pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
  #else //electric
    pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
  #endif
  //KdTree<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>());
  tree->setInputCloud(ref_out);

  // Optional surface smoothing
  if(fpfh_mls_enable_)
  {
    cout << "FPFH: MLS (with " << ref_out->points.size() << " points)" << endl;

    #ifdef PCL_VERSION_COMPARE
      std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
      exit(0);
    #else
      MovingLeastSquares<PointXYZRGB, Normal> mls;
      mls.setInputCloud(ref_out);
      mls.setOutputNormals(n);
      mls.setPolynomialFit(true);
      mls.setPolynomialOrder(2);
      mls.setSearchMethod(tree);
      mls.setSearchRadius(fpfh_rn_);
      mls.reconstruct(*ref_out);
    #endif
    cout << "FPFH: flip normals (with " << ref_out->points.size() << " points)" << endl;
    for (size_t i = 0; i < ref_out->points.size(); ++i)
    {
      flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
                                 n->points[i].normal[0],
                                 n->points[i].normal[1],
                                 n->points[i].normal[2]);
    }
  }
  else
  {
    cout << "FPFH: Normals (with " << ref_out->points.size() << " points)" << endl;
    NormalEstimation<PointXYZRGB, Normal> norm;
    norm.setInputCloud(ref_out);
    norm.setSearchMethod(tree);
    norm.setRadiusSearch(fpfh_rn_);
    norm.compute(*n);
  }

  // FPFH estimation
  #ifdef PCL_VERSION_COMPARE //fuerte
    tree.reset(new pcl::search::KdTree<PointXYZRGB>());
  #else //electric
    tree.reset(new KdTreeFLANN<PointXYZRGB> ());
  #endif
  tree->setInputCloud(ref_out);
  cout << "FPFH: estimation (with " << ref_out->points.size() << " points)" << endl;
  FPFHEstimation<PointXYZRGB, Normal, FPFHSignature33> fpfhE;
  fpfhE.setInputCloud(ref_out);
  fpfhE.setInputNormals(n);
  fpfhE.setSearchMethod(tree);
  fpfhE.setRadiusSearch(fpfh_rf_);
  fpfhE.compute(*fpfh);

  cout << "FPFH: classification " << endl;
  *fpfh_out = *ref_out;

  CvSVM svm;
  svm.load(fpfh_svm_model_.c_str());
  cv::Mat fpfh_histo(1, 33, CV_32FC1);

  int exp_rgb, pre_rgb, predict;
  cob_3d_mapping_common::LabelResults stats(fl2label(fpfh_rn_),fl2label(fpfh_rf_),fpfh_mls_enable_);
  for (size_t idx = 0; idx < ref_out->points.size(); idx++)
  {
    exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
    memcpy(fpfh_histo.ptr<float>(0), fpfh->points[idx].histogram, sizeof(fpfh->points[idx].histogram));
    predict = (int)svm.predict(fpfh_histo);
    //cout << predict << endl;
    switch(predict)
    {
    case SVM_PLANE:
      pre_rgb = LBL_PLANE;
      if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
      break;
    case SVM_EDGE:
      pre_rgb = LBL_EDGE;
      if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
      if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
      break;
    case SVM_COR:
      pre_rgb = LBL_COR;
      if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
      if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
      break;
    case SVM_SPH:
      pre_rgb = LBL_SPH;
      if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
      if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
      break;
    case SVM_CYL:
      pre_rgb = LBL_CYL;
      if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
      if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
      break;
    default:
      pre_rgb = LBL_UNDEF;
      break;
    }

    switch(exp_rgb)
    {
    case LBL_PLANE:
      if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
      stats.exp[EVAL_PLANE]++;
      break;
    case LBL_EDGE:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_EDGE]++;
	if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
      }
      stats.exp[EVAL_EDGE]++;
      stats.exp[EVAL_EDGECORNER]++;
      break;
    case LBL_COR:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_COR]++;
	if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
      }
      stats.exp[EVAL_COR]++;
      stats.exp[EVAL_EDGECORNER]++;
      break;
    case LBL_SPH:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_SPH]++;
	if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
      }
      stats.exp[EVAL_SPH]++;
      stats.exp[EVAL_CURVED]++;
      break;
    case LBL_CYL:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_CYL]++;
	if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
      }
      stats.exp[EVAL_CYL]++;
      stats.exp[EVAL_CURVED]++;
      break;
    default:
      stats.undef++;
      break;
    }
    fpfh_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
  }
  cout << "FPFH:\n" << stats << endl << endl;
}
TEST (PCL, SpinImageEstimation)
{
  // Estimate normals first
  double mr = 0.002;
  NormalEstimation<PointXYZ, Normal> n;
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  // set parameters
  n.setInputCloud (cloud.makeShared ());
  boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
  n.setIndices (indicesptr);
  n.setSearchMethod (tree);
  n.setRadiusSearch (20 * mr);
  n.compute (*normals);

  EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
  EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
  EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);

  EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);

  typedef Histogram<153> SpinImage;
  SpinImageEstimation<PointXYZ, Normal, SpinImage> spin_est(8, 0.5, 16);
  // set parameters
  //spin_est.setInputWithNormals (cloud.makeShared (), normals);
  spin_est.setInputCloud (cloud.makeShared ());
  spin_est.setInputNormals (normals);
  spin_est.setIndices (indicesptr);
  spin_est.setSearchMethod (tree);
  spin_est.setRadiusSearch (40*mr);

  // Object
  PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());


  // radial SI
  spin_est.setRadialStructure();

  // estimate
  spin_est.compute (*spin_images);
  EXPECT_EQ (spin_images->points.size (), indices.size ());

  EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[24], 0.00233226, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[48], 8.48662e-005, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0266387, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0414662, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[144], 0.0128513, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[24], 0.00932424, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[60], 0.0145733, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[96], 0.00034457, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[144], 0.0121195, 1e-4);

  // radial SI, angular spin-images
  spin_est.setAngularDomain ();

  // estimate
  spin_est.compute (*spin_images);
  EXPECT_EQ (spin_images->points.size (), indices.size ());

  EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[48], 0.908814, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[60], 0.63875, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[96], 0.550392, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[144], 0.257136, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[24], 0.230605, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[60], 0.764872, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[96], 1.02824, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[144], 0.293567, 1e-4);

  // rectangular SI
  spin_est.setRadialStructure (false);
  spin_est.setAngularDomain (false);

  // estimate
  spin_est.compute (*spin_images);
  EXPECT_EQ (spin_images->points.size (), indices.size ());

  EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[24], 0.000889345, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0489534, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0747141, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[132], 0.0173423, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[24], 0.0267132, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[108], 0.0209709, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[144], 0.029372, 1e-4);

  // rectangular SI, angular spin-images
  spin_est.setAngularDomain ();

  // estimate
  spin_est.compute (*spin_images);
  EXPECT_EQ (spin_images->points.size (), indices.size ());

  EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[60], 0.38800787925720215, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[96], 0.468881, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[132], 0.67901438474655151, 1e-4);
  EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[24], 0.143845, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[108], 0.706084, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
  EXPECT_NEAR (spin_images->points[300].histogram[144], 0.272542, 1e-4);
}
  TEST (PCL, SpinImageEstimationEigen)
  {
    // Estimate normals first
    double mr = 0.002;
    NormalEstimation<PointXYZ, Normal> n;
    PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
    // set parameters
    n.setInputCloud (cloud.makeShared ());
    boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
    n.setIndices (indicesptr);
    n.setSearchMethod (tree);
    n.setRadiusSearch (20 * mr);
    n.compute (*normals);

    EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
    EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
    EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
    EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
    EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
    EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);

    EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
    EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
    EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);

    SpinImageEstimation<PointXYZ, Normal, Eigen::MatrixXf> spin_est (8, 0.5, 16);
    // set parameters
    //spin_est.setInputWithNormals (cloud.makeShared (), normals);
    spin_est.setInputCloud (cloud.makeShared ());
    spin_est.setInputNormals (normals);
    spin_est.setIndices (indicesptr);
    spin_est.setSearchMethod (tree);
    spin_est.setRadiusSearch (40*mr);

    // Object
    PointCloud<Eigen::MatrixXf>::Ptr spin_images (new PointCloud<Eigen::MatrixXf>);

    // radial SI
    spin_est.setRadialStructure ();

    // estimate
    spin_est.computeEigen (*spin_images);
    EXPECT_EQ (spin_images->points.rows (), indices.size ());

    EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 24), 0.00233226, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 48), 8.48662e-005, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 60), 0.0266387, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 96), 0.0414662, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 144), 0.0128513, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 24), 0.00932424, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 60), 0.0145733, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 96), 0.00034457, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 144), 0.0121195, 1e-4);


    // radial SI, angular spin-images
    spin_est.setAngularDomain ();

    // estimate
    spin_est.computeEigen (*spin_images);
    EXPECT_EQ (spin_images->points.rows (), indices.size ());

    EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 24), 0.13213, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 48), 0.908804, 1.1e-4);
    EXPECT_NEAR (spin_images->points (100, 60), 0.63875, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 96), 0.550392, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 144), 0.25713, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 24), 0.230605, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 60), 0.764872, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 96), 1.02824, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 144), 0.293567, 1e-4);


    // rectangular SI
    spin_est.setRadialStructure (false);
    spin_est.setAngularDomain (false);

    // estimate
    spin_est.computeEigen (*spin_images);
    EXPECT_EQ (spin_images->points.rows (), indices.size ());

    EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 24), 0.000889345, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 60), 0.0489534, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 96), 0.0747141, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 132), 0.0173423, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 24), 0.0267132, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 108), 0.0209709, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 144), 0.029372, 1e-4);

    // rectangular SI, angular spin-images
    spin_est.setAngularDomain ();

    // estimate
    spin_est.computeEigen (*spin_images);
    EXPECT_EQ (spin_images->points.rows (), indices.size ());

    EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 24), 0.132126, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 60), 0.388011, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 96), 0.468881, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 132), 0.678995, 1e-4);
    EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 24), 0.143845, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 108), 0.706084, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
    EXPECT_NEAR (spin_images->points (300, 144), 0.272542, 1e-4);
  }
void processRSD(const PointCloud<PointXYZRGB>::Ptr in,
		PointCloud<PointXYZRGB>::Ptr ref_out,
		PointCloud<PointXYZRGB>::Ptr rsd_out)
{
  PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
  PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>());

  // passthrough filtering (needed to remove NaNs)
  cout << "RSD: Pass (with " << in->points.size() << " points)" << endl;
  PassThrough<PointXYZRGB> pass;
  pass.setInputCloud(in);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(0.0f, pass_depth_);
  pass.filter(*ref_out);

  // optional voxelgrid filtering
  if (rsd_vox_enable_)
  {
    cout << "RSD: Voxel (with " << ref_out->points.size() << " points)" << endl;
    VoxelGrid<PointXYZRGB> vox;
    vox.setInputCloud(ref_out);
    vox.setLeafSize(rsd_vox_, rsd_vox_, rsd_vox_);
    vox.filter(*ref_out);
  }

  #ifdef PCL_VERSION_COMPARE //fuerte
    pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
  #else //electric
    KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
  #endif
  tree->setInputCloud(ref_out);

  // optional surface smoothing
  if(rsd_mls_enable_)
  {
    cout << "RSD: MLS (with " << ref_out->points.size() << " points)" << endl;

    #ifdef PCL_VERSION_COMPARE
      std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
      exit(0);
    #else
      MovingLeastSquares<PointXYZRGB, Normal> mls;
      mls.setInputCloud(ref_out);
      mls.setOutputNormals(n);
      mls.setPolynomialFit(true);
      mls.setPolynomialOrder(2);
      mls.setSearchMethod(tree);
      mls.setSearchRadius(rsd_rn_);
      mls.reconstruct(*ref_out);
    #endif

    cout << "RSD: flip normals (with " << ref_out->points.size() << " points)" << endl;
    for (size_t i = 0; i < ref_out->points.size(); ++i)
    {
      flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
				 n->points[i].normal[0],
				 n->points[i].normal[1],
				 n->points[i].normal[2]);
    }
  }
  else
  {
    cout << "RSD: Normals (with " << ref_out->points.size() << " points)" << endl;
    NormalEstimation<PointXYZRGB, Normal> norm;
    norm.setInputCloud(ref_out);
    norm.setSearchMethod(tree);
    norm.setRadiusSearch(rsd_rn_);
    norm.compute(*n);
  }

  tree->setInputCloud(ref_out);

  // RSD estimation
  cout << "RSD: estimation (with " << ref_out->points.size() << " points)" << endl;
  RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsdE;
  rsdE.setInputCloud(ref_out);
  rsdE.setInputNormals(n);
  rsdE.setSearchMethod(tree);
  rsdE.setPlaneRadius(r_limit_);
  rsdE.setRadiusSearch(rsd_rf_);
  rsdE.compute(*rsd);

  cout << "RSD: classification " << endl;
  *rsd_out = *ref_out;

  // apply RSD rules for classification
  int exp_rgb, pre_rgb;
  float r_max,r_min;
  cob_3d_mapping_common::LabelResults stats(fl2label(rsd_rn_),fl2label(rsd_rf_),rsd_mls_enable_);
  for (size_t idx = 0; idx < ref_out->points.size(); idx++)
  {
    exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
    r_max = rsd->points[idx].r_max;
    r_min = rsd->points[idx].r_min;

    if ( r_min > r_high )
    {
      pre_rgb = LBL_PLANE;
      if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
    }
    else if (r_min < r_low)
    {
      if (r_max < r_r2 * r_min)
      {
	pre_rgb = LBL_COR;
	if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
      }
      else
      {
	pre_rgb = LBL_EDGE;
	if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
      }
      // special case:  combined class for corner and edge
      if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF)
	stats.fp[EVAL_EDGECORNER]++;
    }
    else
    {
      if (r_max < r_r1 * r_min)
      {
	pre_rgb = LBL_SPH;
	if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
      }
      else
      {
	pre_rgb = LBL_CYL;
	if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
      }
      // special case:  combined class for sphere and cylinder
      if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF)
	stats.fp[EVAL_CURVED]++;
    }

    switch(exp_rgb)
    {
    case LBL_PLANE:
      if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
      stats.exp[EVAL_PLANE]++;
      break;
    case LBL_EDGE:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_EDGE]++;
	if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
      }
      stats.exp[EVAL_EDGE]++;
      stats.exp[EVAL_EDGECORNER]++;
      break;
    case LBL_COR:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_COR]++;
	if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
      }
      stats.exp[EVAL_COR]++;
      stats.exp[EVAL_EDGECORNER]++;
      break;
    case LBL_SPH:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_SPH]++;
	if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
      }
      stats.exp[EVAL_SPH]++;
      stats.exp[EVAL_CURVED]++;
      break;
    case LBL_CYL:
      if (pre_rgb != exp_rgb)
      {
	stats.fn[EVAL_CYL]++;
	if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
      }
      stats.exp[EVAL_CYL]++;
      stats.exp[EVAL_CURVED]++;
      break;
    default:
      stats.undef++;
      break;
    }
    rsd_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
  }
  cout << "RSD:\n" << stats << endl << endl;
}
int main(int argc, char** argv)
{
  readOptions(argc, argv);
  boost::timer t;

  // 3D point clouds
  PointCloud<PointXYZRGB>::Ptr p(new PointCloud<PointXYZRGB>);
  PointCloud<Normal>::Ptr n(new PointCloud<Normal>);
  PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>);
  PointCloud<PointLabel>::Ptr l(new PointCloud<PointLabel>);

  PCDReader r;
  if (r.read(file_in_, *p) == -1) return(0);

  vector<PointIndices> indices;
  ifstream fs;

  string line;
  fs.open(file_cluster_.c_str());
  if (fs.is_open())
  {
    while(fs.good())
    {
      getline (fs,line);
      istringstream iss(line);
      PointIndices pi;
      do
      {
        int temp;
        iss >> temp;
        pi.indices.push_back(temp);
      } while(iss);
      if(pi.indices.size()>0)
        indices.push_back(pi);
    }
  }
  std::cout << "indices file read successfully" << std::endl;

  // --- Normal Estimation ---
  if (en_one_)
  {
    t.restart();
    cob_3d_features::OrganizedNormalEstimation<PointXYZRGB, Normal, PointLabel>one;
    one.setInputCloud(p);
    one.setOutputLabels(l);
    //one.setPixelSearchRadius(pns_n_,points_,circle_); //radius,pixel,circle
    one.setPixelSearchRadius(8,2,2);
    one.setSkipDistantPointThreshold(12);
    one.compute(*n);
    cout << t.elapsed() << "s\t for Organized Normal Estimation" << endl;
  }
  else
  {
    t.restart();
    #ifdef PCL_VERSION_COMPARE //fuerte
      pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
    #else //electric
      pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
    #endif
    NormalEstimation<PointXYZRGB, Normal> ne;
    ne.setRadiusSearch(rn_);
    ne.setSearchMethod(tree);
    ne.setInputCloud(p);
    ne.compute(*n);
    cout << t.elapsed() << "s\t for normal estimation" << endl;
  }
  cob_3d_features::OrganizedCurvatureEstimationOMP<PointXYZRGB, Normal, PointLabel, PrincipalCurvatures> oce;
  oce.setInputCloud(p);
  oce.setInputNormals(n);
  oce.setPixelSearchRadius(8,2,2);
  oce.setSkipDistantPointThreshold(12);

  //KdTreeFLANN<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>);
  //ne.setRadiusSearch(rn_);
  //ne.setSearchMethod(tree);
  if (indices.size() == 0)
  {
    oce.setOutputLabels(l);
    oce.compute(*pc);
    cob_3d_features::CurvatureClassifier<PrincipalCurvatures, PointLabel>cc;
    cc.setInputCloud(pc);
    cc.classify(*l);
  }
  else
  {
    for(unsigned int i=0; i<indices.size(); i++)
    {
      std::cout << "cluster " << i << " has " << indices[i].indices.size() << " points" << std::endl;
      if(i==3)
      {
        /*for(unsigned int j=0; j<indices[i].indices.size(); j++)
          std::cout << indices[i].indices[j] << ",";
          std::cout << std::endl;*/
        //cout << i << ": " << indices[i].indices.front() << "!" << endl;
        t.restart();
        boost::shared_ptr<PointIndices> ind_ptr = boost::make_shared<PointIndices>(indices[i]);
        std::cout << ind_ptr->indices.size() << std::endl;
        oce.setIndices(ind_ptr);
        oce.setOutputLabels(l);
        oce.compute(*pc);
        cout << t.elapsed() << "s\t for principal curvature estimation" << endl;

        cob_3d_features::CurvatureClassifier<PrincipalCurvatures, PointLabel>cc;
        cc.setInputCloud(pc);
        cc.setIndices(ind_ptr);
        cc.classify(*l);
      }
    }
  }
  // colorize edges of 3d point cloud
  for (size_t i = 0; i < l->points.size(); i++)
  {
    //std::cout << l->points[i].label << std::endl;
    if (l->points[i].label == I_UNDEF)
    {
      p->points[i].r = 0;
      p->points[i].g = 0;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_NAN)
    {
      p->points[i].r = 0;
      p->points[i].g = 255;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_EDGE)
    {
      p->points[i].r = 0;
      p->points[i].g = 0;
      p->points[i].b = 255;
    }
    else if (l->points[i].label == I_BORDER)
    {
      p->points[i].r = 255;
      p->points[i].g = 0;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_PLANE)
    {
      p->points[i].r = 255;
      p->points[i].g = 255;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_CYL)
    {
      p->points[i].r = 255;
      p->points[i].g = 0;
      p->points[i].b = 255;
    }
    else
    {
      p->points[i].r = 255;
      p->points[i].g = 255;
      p->points[i].b = 255;
    }
  }

  visualization::PCLVisualizer v;
  v.setBackgroundColor(0,127,127);
  ColorHdlRGB col_hdl(p);
  v.addPointCloud<PointXYZRGB>(p,col_hdl, "segmented");

  while(!v.wasStopped())
  {
    v.spinOnce(100);
    usleep(100000);
  }


  return(0);
}
Exemple #17
0
TEST (PCL, GSHOTWithRTransNoised)
{
  PointCloud<PointNormal>::Ptr cloud_nr (new PointCloud<PointNormal> ());
  PointCloud<PointNormal>::Ptr cloud_rot (new PointCloud<PointNormal> ());
  PointCloud<PointNormal>::Ptr cloud_trans (new PointCloud<PointNormal> ());
  PointCloud<PointNormal>::Ptr cloud_rot_trans (new PointCloud<PointNormal> ());
  PointCloud<PointXYZ>::Ptr cloud_noise (new PointCloud<PointXYZ> ());

  Eigen::Affine3f rot = Eigen::Affine3f::Identity ();
  float rot_x = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
  float rot_y = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
  float rot_z = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
  rot.prerotate (Eigen::AngleAxisf (rot_x * M_PI, Eigen::Vector3f::UnitX ()));
  rot.prerotate (Eigen::AngleAxisf (rot_y * M_PI, Eigen::Vector3f::UnitY ()));
  rot.prerotate (Eigen::AngleAxisf (rot_z * M_PI, Eigen::Vector3f::UnitZ ()));
  //std::cout << "rot = (" << (rot_x * M_PI) << ", " << (rot_y * M_PI) << ", " << (rot_z * M_PI) << ")" << std::endl;

  Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
  float HI = 5.0f;
  float LO = -HI;
  float trans_x = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
  float trans_y = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
  float trans_z = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
  //std::cout << "trans = (" << trans_x << ", " << trans_y << ", " << trans_z << ")" << std::endl;
  trans.translate (Eigen::Vector3f (trans_x, trans_y, trans_z));

  // Estimate normals first
  float mr = 0.002;
  NormalEstimation<PointXYZ, pcl::Normal> n;
  PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
  n.setViewPoint (0.0, 0.0, 1.0);
  n.setInputCloud (cloud.makeShared ());
  n.setRadiusSearch (20 * mr);
  n.compute (*normals1);

  pcl::concatenateFields<PointXYZ, Normal, PointNormal> (cloud, *normals1, *cloud_nr);
  pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_rot, rot);
  pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_trans, trans);
  pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_rot, *cloud_rot_trans, trans);

  add_gaussian_noise (cloud.makeShared (), cloud_noise, 0.005);

  PointCloud<Normal>::Ptr normals_noise (new PointCloud<Normal> ());
  n.setInputCloud (cloud_noise);
  n.compute (*normals_noise);

  PointCloud<Normal>::Ptr normals2 (new PointCloud<Normal> ());
  n.setInputCloud (cloud2.makeShared ());
  n.compute (*normals2);

  PointCloud<Normal>::Ptr normals3 (new PointCloud<Normal> ());
  n.setInputCloud (cloud3.makeShared ());
  n.compute (*normals3);

  // Objects
  // Descriptors for ground truth (using SHOT)
  PointCloud<SHOT352>::Ptr desc01 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc02 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc03 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc04 (new PointCloud<SHOT352> ());
  // Descriptors for test GSHOT
  PointCloud<SHOT352>::Ptr desc1 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc2 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc3 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc4 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc5 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc6 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr desc7 (new PointCloud<SHOT352> ());

  // SHOT352 (global)
  GSHOTEstimation<PointNormal, PointNormal, SHOT352> gshot1;
  gshot1.setInputNormals (cloud_nr);
  gshot1.setInputCloud (cloud_nr);
  gshot1.compute (*desc1);
  // Eigen::Vector4f center_desc1 = gshot.getCentralPoint ();

  gshot1.setInputNormals (cloud_rot);
  gshot1.setInputCloud (cloud_rot);
  gshot1.compute (*desc2);
  // Eigen::Vector4f center_desc2 = gshot.getCentralPoint ();

  gshot1.setInputNormals (cloud_trans);
  gshot1.setInputCloud (cloud_trans);
  gshot1.compute (*desc3);
  // Eigen::Vector4f center_desc3 = gshot.getCentralPoint ();

  gshot1.setInputNormals (cloud_rot_trans);
  gshot1.setInputCloud (cloud_rot_trans);
  gshot1.compute (*desc4);
  // Eigen::Vector4f center_desc4 = gshot.getCentralPoint ();

  GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot2;
  gshot2.setInputNormals (normals1);
  gshot2.setInputCloud (cloud_noise);
  gshot2.compute (*desc5);

  gshot2.setInputNormals (normals2);
  gshot2.setInputCloud (cloud2.makeShared ());
  gshot2.compute (*desc6);

  gshot2.setInputNormals (normals3);
  gshot2.setInputCloud (cloud3.makeShared ());
  gshot2.compute (*desc7);

  // Eigen::Vector3f distance_desc = (center_desc3.head<3> () - center_desc1.head<3> ());
  // std::cout << "dist of desc0 and desc3 -> (" << distance_desc[0] << ", " << distance_desc[1] << ", " << distance_desc[2] << ")\n";

  // SHOT352 (local)
  GSHOTEstimation<PointNormal, PointNormal, SHOT352> shot;
  shot.setInputNormals (cloud_nr);
  shot.setInputCloud (ground_truth.makeShared());
  shot.setSearchSurface (cloud_nr);
  shot.setRadiusSearch (radius_local_shot);
  shot.compute (*desc01);

  shot.setInputNormals (cloud_rot);
  shot.setInputCloud (ground_truth.makeShared());
  shot.setSearchSurface (cloud_rot);
  shot.setRadiusSearch (radius_local_shot);
  shot.compute (*desc02);

  shot.setInputNormals (cloud_trans);
  shot.setInputCloud (ground_truth.makeShared());
  shot.setSearchSurface (cloud_trans);
  shot.setRadiusSearch (radius_local_shot);
  shot.compute (*desc03);

  shot.setInputNormals (cloud_rot_trans);
  shot.setInputCloud (ground_truth.makeShared());
  shot.setSearchSurface (cloud_rot_trans);
  shot.setRadiusSearch (radius_local_shot);
  shot.compute (*desc04);

  // CHECK GSHOT
  checkDesc(*desc01, *desc1);
  checkDesc(*desc02, *desc2);
  checkDesc(*desc03, *desc3);
  checkDesc(*desc04, *desc4);

  std::vector<float> d0, d1, d2, d3, d4, d5, d6;
  for(int i = 0; i < 352; ++i)
  {
    d0.push_back(desc1->points[0].descriptor[i]);
    d1.push_back(desc2->points[0].descriptor[i]);
    d2.push_back(desc3->points[0].descriptor[i]);
    d3.push_back(desc4->points[0].descriptor[i]);
    d4.push_back(desc5->points[0].descriptor[i]);
    d5.push_back(desc6->points[0].descriptor[i]);
    d6.push_back(desc7->points[0].descriptor[i]);
  }

  float dist_0 = pcl::selectNorm< std::vector<float> > (d0, d0, 352, pcl::HIK) ;
  float dist_1 = pcl::selectNorm< std::vector<float> > (d0, d1, 352, pcl::HIK) ;
  float dist_2 = pcl::selectNorm< std::vector<float> > (d0, d2, 352, pcl::HIK) ;
  float dist_3 = pcl::selectNorm< std::vector<float> > (d0, d3, 352, pcl::HIK) ;
  float dist_4 = pcl::selectNorm< std::vector<float> > (d0, d4, 352, pcl::HIK) ;
  float dist_5 = pcl::selectNorm< std::vector<float> > (d0, d5, 352, pcl::HIK) ;
  float dist_6 = pcl::selectNorm< std::vector<float> > (d0, d6, 352, pcl::HIK) ;
  
  std::cout << ">> Itself[HIK]:      " << dist_0 << std::endl
            << ">> Rotation[HIK]:    " << dist_1 << std::endl
            << ">> Translate[HIK]:   " << dist_2 << std::endl
            << ">> Rot+Trans[HIK]    " << dist_3 << std::endl
            << ">> GaussNoise[HIK]:  " << dist_4 << std::endl
            << ">> bun03[HIK]:       " << dist_5 << std::endl
            << ">> milk[HIK]:        " << dist_6 << std::endl;

  float high_barrier = dist_0 * 0.999f;
  float noise_barrier = dist_0 * 0.75f;
  float cut_barrier = dist_0 * 0.20f;
  float low_barrier = dist_0 * 0.02f;

  EXPECT_GT (dist_1, high_barrier);
  EXPECT_GT (dist_2, high_barrier);
  //EXPECT_GT (dist_3, high_barrier);
  EXPECT_GT (dist_4, noise_barrier);
  EXPECT_GT (dist_5, cut_barrier);
  EXPECT_LT (dist_6, low_barrier);
}
Exemple #18
0
TEST (PCL, GSHOTShapeEstimation)
{
  // Estimate normals first
  double mr = 0.002;
  NormalEstimation<PointXYZ, Normal> n;
  boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
  n.setInputCloud (cloud.makeShared ());
  n.setIndices (indicesptr);
  n.setSearchMethod (tree);
  n.setRadiusSearch (20 * mr);
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  n.compute (*normals);

  EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
  EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
  EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
  EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
  EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);

  // Objects
  PointCloud<SHOT352>::Ptr gshots352 (new PointCloud<SHOT352> ());
  PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ());
  
  // SHOT352 (local)
  SHOTEstimation<PointXYZ, Normal, SHOT352> shot352;
  shot352.setInputNormals (normals);
  shot352.setRadiusSearch (radius_local_shot);
  shot352.setInputCloud (cloud_for_lrf.makeShared ());
  boost::shared_ptr<vector<int> > indices_local_shot_ptr (new vector<int> (indices_local_shot));
  shot352.setIndices (indices_local_shot_ptr);
  shot352.setSearchSurface (cloud.makeShared());
  shot352.compute (*shots352);

  EXPECT_NEAR (shots352->points[0].descriptor[9 ], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[10], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[11], 0.317935f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[19], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[20], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[21], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[42], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[53], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[54], 0.0f, 1E-4);
  EXPECT_NEAR (shots352->points[0].descriptor[55], 0.089004f, 1E-4);

  // SHOT352 (global)
  GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot352;
  gshot352.setSearchMethod (tree);

  gshot352.setInputNormals (normals);
  EXPECT_EQ (gshot352.getInputNormals (), normals);

  // set parameters
  gshot352.setInputCloud (cloud.makeShared ());
  gshot352.setIndices (indicesptr);

  // estimate
  int gshot_size = 1;
  gshot352.compute (*gshots352);
  EXPECT_EQ (gshots352->points.size (), gshot_size);

  checkDescNear (*gshots352, *shots352, 1E-7);
}