C++ NormalEstimation::setRadiusSearch方法代码示例

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

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

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

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

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

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

/* ---[ */
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);

//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

  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);
//.........这里部分代码省略.........

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]++;
    }
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........