Ejemplo n.º 1
0
TEST (PCL, CVFHEstimation)
{
  // 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);
  CVFHEstimation<PointXYZ, Normal, VFHSignature308> cvfh;
  cvfh.setInputNormals (normals);

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

  // set parameters
  cvfh.setInputCloud (cloud.makeShared ());
  cvfh.setIndices (indicesptr);
  cvfh.setSearchMethod (tree);

  // estimate
  cvfh.compute (*vfhs);
  EXPECT_EQ (static_cast<int>(vfhs->points.size ()), 1);
}
Ejemplo n.º 2
0
TEST (PCL, VFHEstimation)
{
  // 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);
  VFHEstimation<PointXYZ, Normal, VFHSignature308> vfh;
  vfh.setInputNormals (normals);

  //  PointCloud<PointNormal> cloud_normals;
  //  concatenateFields (cloud, normals, cloud_normals);
  //  savePCDFile ("bun0_n.pcd", cloud_normals);

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

  // set parameters
  vfh.setInputCloud (cloud.makeShared ());
  vfh.setIndices (indicesptr);
  vfh.setSearchMethod (tree);

  // estimate
  vfh.compute (*vfhs);
  EXPECT_EQ (int (vfhs->points.size ()), 1);

  //for (size_t d = 0; d < 308; ++d)
  //  std::cerr << vfhs.points[0].histogram[d] << std::endl;
}
Ejemplo n.º 3
0
/* ---[ */
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 ());
}
Ejemplo n.º 4
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);
}
Ejemplo n.º 5
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);
}
Ejemplo n.º 6
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);
}
Ejemplo n.º 7
0
    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);
      }
    }
Ejemplo n.º 8
0
void SurfaceTriangulation::perform(int ksearch)
{
    PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
    PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
    search::KdTree<PointXYZ>::Ptr tree;
    search::KdTree<PointNormal>::Ptr tree2;

    cloud->reserve(myPoints.size());
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
    }

    // Create search tree
    tree.reset (new search::KdTree<PointXYZ> (false));
    tree->setInputCloud (cloud);

    // Normal estimation
    NormalEstimation<PointXYZ, Normal> n;
    PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
    n.setInputCloud (cloud);
    //n.setIndices (indices[B);
    n.setSearchMethod (tree);
    n.setKSearch (ksearch);
    n.compute (*normals);

    // Concatenate XYZ and normal information
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

    // Create search tree
    tree2.reset (new search::KdTree<PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    // Init objects
    GreedyProjectionTriangulation<PointNormal> gp3;

    // Set parameters
    gp3.setInputCloud (cloud_with_normals);
    gp3.setSearchMethod (tree2);
    gp3.setSearchRadius (searchRadius);
    gp3.setMu (mu);
    gp3.setMaximumNearestNeighbors (100);
    gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
    gp3.setMinimumAngle(M_PI/18); // 10 degrees
    gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
    gp3.setNormalConsistency(false);
    gp3.setConsistentVertexOrdering(true);

    // Reconstruct
    PolygonMesh mesh;
    gp3.reconstruct (mesh);

    MeshConversion::convert(mesh, myMesh);

    // Additional vertex information
    //std::vector<int> parts = gp3.getPartIDs();
    //std::vector<int> states = gp3.getPointStates();
}
Ejemplo n.º 9
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);
}
Ejemplo n.º 10
0
void FeatSegment::segment(const PointCloud<PointXYZRGB >::Ptr cloud,  PointCloud<PointXYZRGB >::Ptr outcloud)
{
	// PARAM - Lots of them
	int min_pts_per_cluster = 10;
	int max_pts_per_cluster = INT_MAX; //3000000;
	assert(max_pts_per_cluster > 3000000); // Avoid overflow
    int number_neighbours = 50; // PARAM
    float radius = 0.025; // 0.025 PARAM
    float angle = 0.52; // PARAM

	// Required KdTrees
    pcl::search::KdTree<PointXYZRGB >::Ptr NormalsTree(new pcl::search::KdTree<PointXYZRGB >);
    pcl::search::KdTree<PointXYZRGB >::Ptr ClustersTree(new pcl::search::KdTree<PointXYZRGB >);
    PointCloud<Normal >::Ptr CloudNormals(new PointCloud<Normal >);
    NormalEstimation<PointXYZRGB, Normal > NormalsFinder;
    std::vector<PointIndices > clusters;

	ClustersTree->setInputCloud(cloud);

	// Set normal estimation parameters
    NormalsFinder.setKSearch(number_neighbours);
    NormalsFinder.setSearchMethod(NormalsTree);
    NormalsFinder.setInputCloud(cloud);
    NormalsFinder.compute(*CloudNormals);

    extractEuclideanClusters(cloud, CloudNormals, ClustersTree, radius, clusters, angle, min_pts_per_cluster, max_pts_per_cluster);
    fprintf(stderr, "Number of clusters found matching the given constraints: %d.", (int)clusters.size ());

	std::ofstream FileStr2;
	FileStr2.open("live_segments.txt", ios::app); // NOTE: Don't add the ios:trunc flag here!
	// Copy to clusters to segments
	for (size_t i = 0; i < clusters.size (); ++i)
	{
		if(FileStr2.is_open())
		{
			for(int j = 0; j < clusters[i].indices.size(); ++j)
			{
				if(j == clusters[i].indices.size() - 1)
				{
					FileStr2 << clusters[i].indices.at(j) << endl;
					continue; // Also break;
				}
				FileStr2 << clusters[i].indices.at(j) << " ";
			}

			FeatPointCloud * Segment = new FeatPointCloud(m_SceneCloud, clusters[i].indices, m_ConfigFName);
			m_Segments.push_back(Segment);
		}
		else
			cout << "Failed to open file.\n";
	}
	FileStr2.close();
}
Ejemplo n.º 11
0
void PoissonReconstruction::perform(int ksearch)
{
    PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
    PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
    search::KdTree<PointXYZ>::Ptr tree;
    search::KdTree<PointNormal>::Ptr tree2;

    cloud->reserve(myPoints.size());
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
    }

    // Create search tree
    tree.reset (new search::KdTree<PointXYZ> (false));
    tree->setInputCloud (cloud);

    // Normal estimation
    NormalEstimation<PointXYZ, Normal> n;
    PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
    n.setInputCloud (cloud);
    //n.setIndices (indices[B);
    n.setSearchMethod (tree);
    n.setKSearch (ksearch);
    n.compute (*normals);

    // Concatenate XYZ and normal information
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

    // Create search tree
    tree2.reset (new search::KdTree<PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    // Init objects
    Poisson<PointNormal> poisson;

    // Set parameters
    poisson.setInputCloud (cloud_with_normals);
    poisson.setSearchMethod (tree2);
    if (depth >= 1)
        poisson.setDepth(depth);
    if (solverDivide >= 1)
        poisson.setSolverDivide(solverDivide);
    if (samplesPerNode >= 1.0f)
        poisson.setSamplesPerNode(samplesPerNode);

    // Reconstruct
    PolygonMesh mesh;
    poisson.reconstruct (mesh);

    MeshConversion::convert(mesh, myMesh);
}
Ejemplo n.º 12
0
 /** estimate the normals of a point cloud */
 static PointCloud<Normal>::Ptr
 compute_pcn(PointCloud<PointXYZ>::ConstPtr in, float vx, float vy, float vz)
 {
   PointCloud<Normal>::Ptr pcn (new PointCloud<Normal>());
   NormalEstimation<PointXYZ, Normal> ne;
   search::KdTree<PointXYZ>::Ptr kdt (new search::KdTree<PointXYZ>());
   ne.setInputCloud(in);
   ne.setSearchMethod(kdt);
   ne.setKSearch(20);
   ne.setViewPoint(vx, vy, vz);
   ne.compute(*pcn);
   return pcn;
 }
Ejemplo n.º 13
0
void Reen::MarchingCubesRBF::perform(int ksearch)
{
    PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
    PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
    search::KdTree<PointXYZ>::Ptr tree;
    search::KdTree<PointNormal>::Ptr tree2;

    cloud->reserve(myPoints.size());
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
    }

    // Create search tree
    tree.reset (new search::KdTree<PointXYZ> (false));
    tree->setInputCloud (cloud);

    // Normal estimation
    NormalEstimation<PointXYZ, Normal> n;
    PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
    n.setInputCloud (cloud);
    //n.setIndices (indices[B);
    n.setSearchMethod (tree);
    n.setKSearch (ksearch);
    n.compute (*normals);

    // Concatenate XYZ and normal information
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

    // Create search tree
    tree2.reset (new search::KdTree<PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    // Init objects
    pcl::MarchingCubesRBF<PointNormal> rbf;

    // Set parameters
    rbf.setIsoLevel (0);
    rbf.setGridResolution (60, 60, 60);
    rbf.setPercentageExtendGrid (0.1f);
    rbf.setOffSurfaceDisplacement (0.02f);

    rbf.setInputCloud (cloud_with_normals);
    rbf.setSearchMethod (tree2);

    // Reconstruct
    PolygonMesh mesh;
    rbf.reconstruct (mesh);

    MeshConversion::convert(mesh, myMesh);
}
Ejemplo n.º 14
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 ());
}
Ejemplo n.º 15
0
void GridReconstruction::perform(int ksearch)
{
    PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
    PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
    search::KdTree<PointXYZ>::Ptr tree;
    search::KdTree<PointNormal>::Ptr tree2;

    cloud->reserve(myPoints.size());
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
    }

    // Create search tree
    tree.reset (new search::KdTree<PointXYZ> (false));
    tree->setInputCloud (cloud);

    // Normal estimation
    NormalEstimation<PointXYZ, Normal> n;
    PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
    n.setInputCloud (cloud);
    //n.setIndices (indices[B);
    n.setSearchMethod (tree);
    n.setKSearch (ksearch);
    n.compute (*normals);

    // Concatenate XYZ and normal information
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

    // Create search tree
    tree2.reset (new search::KdTree<PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    // Init objects
    GridProjection<PointNormal> grid;

    // Set parameters
    grid.setResolution(0.005); 
    grid.setPaddingSize(3); 
    grid.setNearestNeighborNum(100); 
    grid.setMaxBinarySearchLevel(10);
    grid.setInputCloud (cloud_with_normals);
    grid.setSearchMethod (tree2);

    // Reconstruct
    PolygonMesh mesh;
    grid.reconstruct (mesh);

    MeshConversion::convert(mesh, myMesh);
}
Ejemplo n.º 16
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);
    }
Ejemplo n.º 17
0
    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);
    };
Ejemplo n.º 18
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);
}
Ejemplo n.º 19
0
void getCylClusters (boost::shared_ptr<PointCloud<T> > sceneCloud, vector<boost::shared_ptr<PointCloud<T> > > &outVector) {

  typedef typename pcl::search::KdTree<T>::Ptr my_KdTreePtr;
  typedef typename pcl::PointCloud<T>::Ptr my_PointCloudPtr;

  NormalEstimation<T, Normal> ne;
  SACSegmentationFromNormals<T, Normal> seg; 
  my_KdTreePtr tree (new pcl::search::KdTree<T> ());
  PointCloud<Normal>::Ptr cloud_normals (new PointCloud<pcl::Normal>);

  ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
  PointIndices::Ptr  inliers_cylinder (new pcl::PointIndices);
  ExtractIndices<T> extract;

  // Estimate point normals
  ne.setSearchMethod (tree);
  ne.setInputCloud (sceneCloud);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);

  seg.setOptimizeCoefficients (true);
  seg.setModelType (SACMODEL_CYLINDER);
  seg.setMethodType (SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.05);
  seg.setRadiusLimits (0, 0.1);
  seg.setInputCloud (sceneCloud);
  seg.setInputNormals (cloud_normals);

  // Obtain the cylinder inliers and coefficients
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  cout << " Number of inliers vs total number of points " << inliers_cylinder->indices.size() << " vs " << sceneCloud->size() << endl; 
  // Write the cylinder inliers to disk
  extract.setInputCloud (sceneCloud);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  my_PointCloudPtr cloud_cylinder (new PointCloud<T> ());
  extract.filter (*cloud_cylinder);
  
  outVector.push_back(cloud_cylinder);
}
Ejemplo n.º 20
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);
}
Ejemplo n.º 21
0
TEST (PCL, NormalEstimation)
{
  tree.reset (new search::KdTree<PointXYZ> (false));
  n.setSearchMethod (tree);
  n.setKSearch (10);

  n.setInputCloud (cloud.makeShared ());

  PointCloud<Normal> output;
  n.compute (output);

  EXPECT_EQ (output.points.size (), cloud.points.size ());
  EXPECT_EQ (output.width, cloud.width);
  EXPECT_EQ (output.height, cloud.height);

  for (size_t i = 0; i < cloud.points.size (); ++i)
  {
    EXPECT_NEAR (fabs (output.points[i].normal_x),   0, 1e-2);
    EXPECT_NEAR (fabs (output.points[i].normal_y),   0, 1e-2);
    EXPECT_NEAR (fabs (output.points[i].normal_z), 1.0, 1e-2);
  }
}
Ejemplo n.º 22
0
TEST (PCL, PrincipalCurvaturesEstimation)
{
    float pcx, pcy, pcz, pc1, pc2;

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

    PrincipalCurvaturesEstimation<PointXYZ, Normal, PrincipalCurvatures> pc;
    pc.setInputNormals (normals);
    EXPECT_EQ (pc.getInputNormals (), normals);

    // computePointPrincipalCurvatures (indices)
    pc.computePointPrincipalCurvatures (*normals, 0, indices, pcx, pcy, pcz, pc1, pc2);
    EXPECT_NEAR (fabs (pcx), 0.98509, 1e-4);
    EXPECT_NEAR (fabs (pcy), 0.10714, 1e-4);
    EXPECT_NEAR (fabs (pcz), 0.13462, 1e-4);
    EXPECT_NEAR (pc1, 0.23997423052787781, 1e-4);
    EXPECT_NEAR (pc2, 0.19400238990783691, 1e-4);

    pc.computePointPrincipalCurvatures (*normals, 2, indices, pcx, pcy, pcz, pc1, pc2);
    EXPECT_NEAR (pcx, 0.98079, 1e-4);
    EXPECT_NEAR (pcy, -0.04019, 1e-4);
    EXPECT_NEAR (pcz, 0.19086, 1e-4);
    EXPECT_NEAR (pc1, 0.27207490801811218, 1e-4);
    EXPECT_NEAR (pc2, 0.19464978575706482, 1e-4);

    int indices_size = static_cast<int> (indices.size ());
    pc.computePointPrincipalCurvatures (*normals, indices_size - 3, indices, pcx, pcy, pcz, pc1, pc2);
    EXPECT_NEAR (pcx, 0.86725, 1e-4);
    EXPECT_NEAR (pcy, -0.37599, 1e-4);
    EXPECT_NEAR (pcz, 0.32635, 1e-4);
    EXPECT_NEAR (pc1, 0.25900053977966309, 1e-4);
    EXPECT_NEAR (pc2, 0.17906945943832397, 1e-4);

    pc.computePointPrincipalCurvatures (*normals, indices_size - 1, indices, pcx, pcy, pcz, pc1, pc2);
    EXPECT_NEAR (pcx, 0.86725, 1e-4);
    EXPECT_NEAR (pcy, -0.375851, 1e-3);
    EXPECT_NEAR (pcz, 0.32636, 1e-4);
    EXPECT_NEAR (pc1, 0.2590005099773407,  1e-4);
    EXPECT_NEAR (pc2, 0.17906956374645233, 1e-4);

    // Object
    PointCloud<PrincipalCurvatures>::Ptr pcs (new PointCloud<PrincipalCurvatures> ());

    // set parameters
    pc.setInputCloud (cloud.makeShared ());
    pc.setIndices (indicesptr);
    pc.setSearchMethod (tree);
    pc.setKSearch (indices_size);

    // estimate
    pc.compute (*pcs);
    EXPECT_EQ (pcs->points.size (), indices.size ());

    // Adjust for small numerical inconsitencies (due to nn_indices not being sorted)
    EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[0]), 0.98509, 1e-4);
    EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[1]), 0.10713, 1e-4);
    EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[2]), 0.13462, 1e-4);
    EXPECT_NEAR (fabs (pcs->points[0].pc1), 0.23997458815574646, 1e-4);
    EXPECT_NEAR (fabs (pcs->points[0].pc2), 0.19400238990783691, 1e-4);

    EXPECT_NEAR (pcs->points[2].principal_curvature[0], 0.98079, 1e-4);
    EXPECT_NEAR (pcs->points[2].principal_curvature[1], -0.04019, 1e-4);
    EXPECT_NEAR (pcs->points[2].principal_curvature[2], 0.19086, 1e-4);
    EXPECT_NEAR (pcs->points[2].pc1, 0.27207502722740173, 1e-4);
    EXPECT_NEAR (pcs->points[2].pc2, 0.1946497857570648,  1e-4);

    EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[0], 0.86725, 1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[1], -0.37599, 1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[2], 0.32636, 1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 3].pc1, 0.2590007483959198,  1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 3].pc2, 0.17906941473484039, 1e-4);

    EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[0], 0.86725, 1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[1], -0.375851, 1e-3);
    EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[2], 0.32636, 1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 1].pc1, 0.25900065898895264, 1e-4);
    EXPECT_NEAR (pcs->points[indices.size () - 1].pc2, 0.17906941473484039, 1e-4);
}
Ejemplo n.º 23
0
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);
}
Ejemplo n.º 24
0
  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);
  }
Ejemplo n.º 25
0
TEST (PCL, FPFHEstimationOpenMP)
{
  // 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);
  FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33> fpfh (4); // instantiate 4 threads
  fpfh.setInputNormals (normals);

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

  // 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<FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33>
  (cloud.makeShared (), normals, test_indices, 33);
}
Ejemplo n.º 26
0
TEST (PCL, BoundaryEstimation)
{
  Eigen::Vector4f u = Eigen::Vector4f::Zero ();
  Eigen::Vector4f v = Eigen::Vector4f::Zero ();

  // 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 (static_cast<int> (indices.size ()));
  // estimate
  n.compute (*normals);

  BoundaryEstimation<PointXYZ, Normal, Boundary> b;
  b.setInputNormals (normals);
  EXPECT_EQ (b.getInputNormals (), normals);

  // getCoordinateSystemOnPlane
  for (size_t i = 0; i < normals->points.size (); ++i)
  {
    b.getCoordinateSystemOnPlane (normals->points[i], u, v);
    Vector4fMap n4uv = normals->points[i].getNormalVector4fMap ();
    EXPECT_NEAR (n4uv.dot(u), 0, 1e-4);
    EXPECT_NEAR (n4uv.dot(v), 0, 1e-4);
    EXPECT_NEAR (u.dot(v), 0, 1e-4);
  }

  // isBoundaryPoint (indices)
  bool pt = false;
  pt = b.isBoundaryPoint (cloud, 0, indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, false);
  pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) / 3, indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, false);
  pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) / 2, indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, false);
  pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) - 1, indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, true);

  // isBoundaryPoint (points)
  pt = false;
  pt = b.isBoundaryPoint (cloud, cloud.points[0], indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, false);
  pt = b.isBoundaryPoint (cloud, cloud.points[indices.size () / 3], indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, false);
  pt = b.isBoundaryPoint (cloud, cloud.points[indices.size () / 2], indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, false);
  pt = b.isBoundaryPoint (cloud, cloud.points[indices.size () - 1], indices, u, v, float (M_PI) / 2.0);
  EXPECT_EQ (pt, true);

  // Object
  PointCloud<Boundary>::Ptr bps (new PointCloud<Boundary> ());

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

  // estimate
  b.compute (*bps);
  EXPECT_EQ (bps->points.size (), indices.size ());

  pt = bps->points[0].boundary_point;
  EXPECT_EQ (pt, false);
  pt = bps->points[indices.size () / 3].boundary_point;
  EXPECT_EQ (pt, false);
  pt = bps->points[indices.size () / 2].boundary_point;
  EXPECT_EQ (pt, false);
  pt = bps->points[indices.size () - 1].boundary_point;
  EXPECT_EQ (pt, true);
}
Ejemplo n.º 27
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);
}
Ejemplo n.º 28
0
TEST (PCL, NormalEstimation)
{
  Eigen::Vector4f plane_parameters;
  float curvature;

  NormalEstimation<PointXYZ, Normal> n;

  // computePointNormal (indices, Vector)
  computePointNormal (cloud, indices, plane_parameters, curvature);
  EXPECT_NEAR (fabs (plane_parameters[0]), 0.035592, 1e-4);
  EXPECT_NEAR (fabs (plane_parameters[1]), 0.369596, 1e-4);
  EXPECT_NEAR (fabs (plane_parameters[2]), 0.928511, 1e-4);
  EXPECT_NEAR (fabs (plane_parameters[3]), 0.0622552, 1e-4);
  EXPECT_NEAR (curvature, 0.0693136, 1e-4);

  float nx, ny, nz;
  // computePointNormal (indices)
  n.computePointNormal (cloud, indices, nx, ny, nz, curvature);
  EXPECT_NEAR (fabs (nx), 0.035592, 1e-4);
  EXPECT_NEAR (fabs (ny), 0.369596, 1e-4);
  EXPECT_NEAR (fabs (nz), 0.928511, 1e-4);
  EXPECT_NEAR (curvature, 0.0693136, 1e-4);

  // computePointNormal (Vector)
  computePointNormal (cloud, plane_parameters, curvature);
  EXPECT_NEAR (plane_parameters[0],  0.035592,  1e-4);
  EXPECT_NEAR (plane_parameters[1],  0.369596,  1e-4);
  EXPECT_NEAR (plane_parameters[2],  0.928511,  1e-4);
  EXPECT_NEAR (plane_parameters[3], -0.0622552, 1e-4);
  EXPECT_NEAR (curvature,            0.0693136, 1e-4);

  // flipNormalTowardsViewpoint (Vector)
  flipNormalTowardsViewpoint (cloud.points[0], 0, 0, 0, plane_parameters);
  EXPECT_NEAR (plane_parameters[0], -0.035592,  1e-4);
  EXPECT_NEAR (plane_parameters[1], -0.369596,  1e-4);
  EXPECT_NEAR (plane_parameters[2], -0.928511,  1e-4);
  EXPECT_NEAR (plane_parameters[3],  0.0799743, 1e-4);

  // flipNormalTowardsViewpoint
  flipNormalTowardsViewpoint (cloud.points[0], 0, 0, 0, nx, ny, nz);
  EXPECT_NEAR (nx, -0.035592, 1e-4);
  EXPECT_NEAR (ny, -0.369596, 1e-4);
  EXPECT_NEAR (nz, -0.928511, 1e-4);

  // Object
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());

  // set parameters
  PointCloud<PointXYZ>::Ptr cloudptr = cloud.makeShared ();
  n.setInputCloud (cloudptr);
  EXPECT_EQ (n.getInputCloud (), cloudptr);
  boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
  n.setIndices (indicesptr);
  EXPECT_EQ (n.getIndices (), indicesptr);
  n.setSearchMethod (tree);
  EXPECT_EQ (n.getSearchMethod (), tree);
  n.setKSearch (static_cast<int> (indices.size ()));

  // estimate
  n.compute (*normals);
  EXPECT_EQ (normals->points.size (), indices.size ());

  for (size_t i = 0; i < normals->points.size (); ++i)
  {
    EXPECT_NEAR (normals->points[i].normal[0], -0.035592, 1e-4);
    EXPECT_NEAR (normals->points[i].normal[1], -0.369596, 1e-4);
    EXPECT_NEAR (normals->points[i].normal[2], -0.928511, 1e-4);
    EXPECT_NEAR (normals->points[i].curvature, 0.0693136, 1e-4);
  }

  PointCloud<PointXYZ>::Ptr surfaceptr = cloudptr;
  n.setSearchSurface (surfaceptr);
  EXPECT_EQ (n.getSearchSurface (), surfaceptr);

  // Additional test for searchForNeigbhors
  surfaceptr.reset (new PointCloud<PointXYZ>);
  *surfaceptr = *cloudptr;
  surfaceptr->points.resize (640 * 480);
  surfaceptr->width = 640;
  surfaceptr->height = 480;
  EXPECT_EQ (surfaceptr->points.size (), surfaceptr->width * surfaceptr->height);
  n.setSearchSurface (surfaceptr);
  tree.reset ();
  n.setSearchMethod (tree);

  // estimate
  n.compute (*normals);
  EXPECT_EQ (normals->points.size (), indices.size ());
}
Ejemplo n.º 29
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);
}
Ejemplo n.º 30
0
/* ---[ */
int
main (int argc, char** argv)
{
  if (argc < 2)
  {
    std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  // Load file
  sensor_msgs::PointCloud2 cloud_blob;
  loadPCDFile (argv[1], cloud_blob);
  fromROSMsg (cloud_blob, *cloud);

  // Create search tree
  tree.reset (new search::KdTree<PointXYZ> (false));
  tree->setInputCloud (cloud);

  // Normal estimation
  NormalEstimation<PointXYZ, Normal> n;
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  n.setInputCloud (cloud);
  //n.setIndices (indices[B);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);

  // Concatenate XYZ and normal information
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
      
  // Create search tree
  tree2.reset (new search::KdTree<PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  // Process for update cloud
  if(argc == 3){
    sensor_msgs::PointCloud2 cloud_blob1;
    loadPCDFile (argv[2], cloud_blob1);
    fromROSMsg (cloud_blob1, *cloud1);
        // Create search tree
    tree3.reset (new search::KdTree<PointXYZ> (false));
    tree3->setInputCloud (cloud1);

    // Normal estimation
    NormalEstimation<PointXYZ, Normal> n1;
    PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
    n1.setInputCloud (cloud1);

    n1.setSearchMethod (tree3);
    n1.setKSearch (20);
    n1.compute (*normals1);

    // Concatenate XYZ and normal information
    pcl::concatenateFields (*cloud1, *normals1, *cloud_with_normals1);
    // Create search tree
    tree4.reset (new search::KdTree<PointNormal>);
    tree4->setInputCloud (cloud_with_normals1);
  }

  // Testing
  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}