Beispiel #1
0
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         int k, double radius)
{
  // Convert data to PointCloud<T>
  PointCloud<PointNormal>::Ptr xyznormals (new PointCloud<PointNormal>);
  fromROSMsg (*input, *xyznormals);

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

  FPFHEstimation<PointNormal, PointNormal, FPFHSignature33> ne;
  ne.setInputCloud (xyznormals);
  ne.setInputNormals (xyznormals);
  ne.setSearchMethod (search::KdTree<PointNormal>::Ptr (new search::KdTree<PointNormal>));
  ne.setKSearch (k);
  ne.setRadiusSearch (radius);
  
  PointCloud<FPFHSignature33> fpfhs;
  ne.compute (fpfhs);

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

  // Convert data back
  sensor_msgs::PointCloud2 output_fpfhs;
  toROSMsg (fpfhs, output_fpfhs);
  concatenateFields (*input, output_fpfhs, output);
}
TEST (PCL, FPFHEstimation)
{
  // Estimate normals first
  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.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
  // estimate
  n.compute (*normals);

  FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh;
  fpfh.setInputNormals (normals);
  EXPECT_EQ (fpfh.getInputNormals (), normals);

  // computePointSPFHSignature
  int nr_subdiv = 11; // use the same number of bins for all three angular features
  Eigen::MatrixXf hist_f1 (indices.size (), nr_subdiv), hist_f2 (indices.size (), nr_subdiv), hist_f3 (indices.size (), nr_subdiv);
  hist_f1.setZero (); hist_f2.setZero (); hist_f3.setZero ();
  for (int i = 0; i < static_cast<int> (indices.size ()); ++i)
    fpfh.computePointSPFHSignature (cloud, *normals, i, i, indices, hist_f1, hist_f2, hist_f3);

  EXPECT_NEAR (hist_f1 (0, 0), 0.757576, 1e-4);
  EXPECT_NEAR (hist_f1 (0, 1), 0.757576, 1e-4);
  EXPECT_NEAR (hist_f1 (0, 2), 4.54545,  1e-4);
  EXPECT_NEAR (hist_f1 (0, 3), 19.697,   1e-4);
  EXPECT_NEAR (hist_f1 (0, 4), 40.6566,  1e-4);
  EXPECT_NEAR (hist_f1 (0, 5), 21.4647,  1e-4);
  EXPECT_NEAR (hist_f1 (0, 6), 7.575759, 1e-4);
  EXPECT_NEAR (hist_f1 (0, 7), 0.000000, 1e-4);
  EXPECT_NEAR (hist_f1 (0, 8), 0.000000, 1e-4);
  EXPECT_NEAR (hist_f1 (0, 9), 0.50505,  1e-4);
  EXPECT_NEAR (hist_f1 (0, 10), 4.0404,  1e-4);

  EXPECT_NEAR (hist_f2 (0, 0), 0.757576, 1e-4);
  EXPECT_NEAR (hist_f2 (0, 1), 1.51515,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 2), 6.31313,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 3), 9.59596,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 4), 20.7071,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 5), 18.9394,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 6), 15.9091,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 7), 12.8788,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 8), 6.56566,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 9), 4.29293,  1e-4);
  EXPECT_NEAR (hist_f2 (0, 10), 2.52525, 1e-4);

  EXPECT_NEAR (hist_f3 (0, 0), 0.000000, 1e-4);
  EXPECT_NEAR (hist_f3 (0, 1), 5.05051,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 2), 4.54545,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 3), 5.05051,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 4), 1.76768,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 5), 3.0303,   1e-4);
  EXPECT_NEAR (hist_f3 (0, 6), 9.09091,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 7), 31.8182,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 8), 22.2222,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 9), 11.8687,  1e-4);
  EXPECT_NEAR (hist_f3 (0, 10), 5.55556, 1e-4);

  // weightPointSPFHSignature
  Eigen::VectorXf fpfh_histogram (nr_subdiv + nr_subdiv + nr_subdiv);
  fpfh_histogram.setZero ();
  vector<float> dists (indices.size ());
  for (size_t i = 0; i < dists.size (); ++i) dists[i] = static_cast<float> (i);
  fpfh.weightPointSPFHSignature (hist_f1, hist_f2, hist_f3, indices, dists, fpfh_histogram);

  EXPECT_NEAR (fpfh_histogram[0],  1.9798 ,  1e-2);
  EXPECT_NEAR (fpfh_histogram[1],  2.86927,  1e-2);
  EXPECT_NEAR (fpfh_histogram[2],  8.47911,  1e-2);
  EXPECT_NEAR (fpfh_histogram[3],  22.8784,  1e-2);
  EXPECT_NEAR (fpfh_histogram[4],  29.8597,  1e-2);
  EXPECT_NEAR (fpfh_histogram[5],  19.6877,  1e-2);
  EXPECT_NEAR (fpfh_histogram[6],  7.38611,  1e-2);
  EXPECT_NEAR (fpfh_histogram[7],  1.44265,  1e-2);
  EXPECT_NEAR (fpfh_histogram[8],  0.69677,  1e-2);
  EXPECT_NEAR (fpfh_histogram[9],  1.72609,  1e-2);
  EXPECT_NEAR (fpfh_histogram[10], 2.99435,  1e-2);
  EXPECT_NEAR (fpfh_histogram[11], 2.26313,  1e-2);
  EXPECT_NEAR (fpfh_histogram[12], 5.16573,  1e-2);
  EXPECT_NEAR (fpfh_histogram[13], 8.3263 ,  1e-2);
  EXPECT_NEAR (fpfh_histogram[14], 9.92427,  1e-2);
  EXPECT_NEAR (fpfh_histogram[15], 16.8062,  1e-2);
  EXPECT_NEAR (fpfh_histogram[16], 16.2767,  1e-2);
  EXPECT_NEAR (fpfh_histogram[17], 12.251 ,  1e-2);
  //EXPECT_NEAR (fpfh_histogram[18], 10.354,  1e-1);
  //EXPECT_NEAR (fpfh_histogram[19], 6.65578,  1e-2);
  EXPECT_NEAR (fpfh_histogram[20], 6.1437 ,  1e-2);
  EXPECT_NEAR (fpfh_histogram[21], 5.83341,  1e-2);
  EXPECT_NEAR (fpfh_histogram[22], 1.08809,  1e-2);
  EXPECT_NEAR (fpfh_histogram[23], 3.34133,  1e-2);
  EXPECT_NEAR (fpfh_histogram[24], 5.59236,  1e-2);
  EXPECT_NEAR (fpfh_histogram[25], 5.6355 ,  1e-2);
  EXPECT_NEAR (fpfh_histogram[26], 3.03257,  1e-2);
  EXPECT_NEAR (fpfh_histogram[27], 1.37437,  1e-2);
  EXPECT_NEAR (fpfh_histogram[28], 7.99746,  1e-2);
  EXPECT_NEAR (fpfh_histogram[29], 18.0343,  1e-2);
  EXPECT_NEAR (fpfh_histogram[30], 23.691 ,  1e-2);
  EXPECT_NEAR (fpfh_histogram[31], 19.8475,  1e-2);
  EXPECT_NEAR (fpfh_histogram[32], 10.3655,  1e-2);

  // Object
  PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ());

  // set parameters
  fpfh.setInputCloud (cloud.makeShared ());
  fpfh.setNrSubdivisions (11, 11, 11);
  fpfh.setIndices (indicesptr);
  fpfh.setSearchMethod (tree);
  fpfh.setKSearch (static_cast<int> (indices.size ()));

  // estimate
  fpfh.compute (*fpfhs);
  EXPECT_EQ (fpfhs->points.size (), indices.size ());

  EXPECT_NEAR (fpfhs->points[0].histogram[0],  1.58591, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[1],  1.68365, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[2],  6.71   , 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[3],  23.0717, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[4],  33.3844, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[5],  20.4002, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[6],  7.31067, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[7],  1.02635, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[8],  0.48591, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[9],  1.47069, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-2);
  //EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-2);
  //EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28565, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73887, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-2);
  EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-2);

  // Test results when setIndices and/or setSearchSurface are used

  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));

  testIndicesAndSearchSurface<FPFHEstimation<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33>
  (cloud.makeShared (), normals, test_indices, 33);

}
/*! @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;
}