C++ search::KdTree类代码示例

int ICP::getNormal(const CloudPtr &cloud_in, NormalCloudPtr &cloud_out, pcl::search::KdTree<Point>::Ptr &tree)
{
    tree->setInputCloud(cloud_in);
    pcl::NormalEstimation<Point, NormalPoint> norm_est;
    norm_est.setSearchMethod(tree);
    norm_est.setKSearch(_GetNormalKSearch);
    norm_est.setInputCloud(cloud_in);
    norm_est.compute(*cloud_out);
    Utils::combineField(cloud_in, cloud_out);
    return 0;
}

void extractEuclideanClusters (
      PointCloud<PointXYZRGB >::Ptr cloud, pcl::PointCloud<pcl::Normal >::Ptr normals,
      pcl::search::KdTree<PointXYZRGB >::Ptr tree, 
      float tolerance, std::vector<pcl::PointIndices > &clusters, double eps_angle,
      unsigned int min_pts_per_cluster = 1,
      unsigned int max_pts_per_cluster = (std::numeric_limits<int >::max) ())
  {
    // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
    //and indices[i]
    float adjTolerance = 0;

    // Create a bool vector of processed point indices, and initialize it to false
    std::vector<bool > processed(cloud->points.size(), false);

	std::vector<int> nn_indices;
	std::vector<float> nn_distances;
	// Process all points in the indices vector
	std::cout << "Point size is " << cloud->points.size () << std::endl;
	for (size_t i = 0; i < cloud->points.size (); ++i)
    {
		if(processed[i])
			continue;

		std::vector<int > seed_queue;
		int sq_idx = 0;
		seed_queue.push_back(i);
		processed[i] = true;

		int cnt = 0;

		while (sq_idx < (int)seed_queue.size())
		{ 
			cnt++;

			// Search for sq_idx
//			 adjTolerance = cloud->points[seed_queue[sq_idx]].distance * tolerance;
			adjTolerance = tolerance;

			if (!tree->radiusSearch(seed_queue[sq_idx], adjTolerance, nn_indices, nn_distances))
	        {
				sq_idx++;
				continue;
			}

			for(size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
			{
				if (processed[nn_indices[j]]) // Has this point been processed before ?
					continue;

				processed[nn_indices[j]] = true;
				// [-1;1]
				double dot_p =
				normals->points[i].normal[0] * normals->points[nn_indices[j]].normal[0] +
				normals->points[i].normal[1] * normals->points[nn_indices[j]].normal[1] +
				normals->points[i].normal[2] * normals->points[nn_indices[j]].normal[2];
				if ( fabs (acos (dot_p)) < eps_angle )
				{
					processed[nn_indices[j]] = true;
					seed_queue.push_back (nn_indices[j]);
				}
	        }

			sq_idx++;
		}

		// If this queue is satisfactory, add to the clusters
		if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
		{
			pcl::PointIndices r;
			r.indices.resize (seed_queue.size ());
			for (size_t j = 0; j < seed_queue.size (); ++j)
				r.indices[j] = seed_queue[j];

			sort (r.indices.begin (), r.indices.end ());
			r.indices.erase (unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

			r.header = cloud->header;
			//ROS_INFO ("cluster of size %d data point\n ",r.indices.size());
			clusters.push_back(r);
		}
	}
}

void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
  int idx = event.getPointIndex ();
  if (idx == -1)
    return;

  if (!cloud)
  {
    cloud = *reinterpret_cast<pcl::PCLPointCloud2::Ptr*> (cookie);
    xyzcloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2 (*cloud, *xyzcloud);
    search.setInputCloud (xyzcloud);
  }
  // Return the correct index in the cloud instead of the index on the screen
  std::vector<int> indices (1);
  std::vector<float> distances (1);

  // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
  pcl::PointXYZ picked_pt;
  event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
  //TODO: Look into this.
  search.nearestKSearch (picked_pt, 1, indices, distances);

  PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z);

  idx = indices[0];
  // If two points were selected, draw an arrow between them
  pcl::PointXYZ p1, p2;
  if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
  {
    std::stringstream ss;
    ss << p1 << p2;
    p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
    return;
  }

  // Else, if a single point has been selected
  std::stringstream ss;
  ss << idx;
  // Get the cloud's fields
  for (size_t i = 0; i < cloud->fields.size (); ++i)
  {
    if (!isMultiDimensionalFeatureField (cloud->fields[i]))
      continue;
    PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
    ph_global.renderOnce ();
#endif
  }
  if (p)
  {
    pcl::PointXYZ pos;
    event.getPoint (pos.x, pos.y, pos.z);
    p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
  }
  
}

void FindPickedPoint(const pcl::visualization::PointPickingEvent& event) {
	int idx = event.getPointIndex ();
	if (idx == -1)
	{
		std::cout << "Invalid pick!\n;";
		return;
	}
	search.setInputCloud(build_cloud_accurate_plane);

	// Return the correct index in the cloud instead of the index on the screen
	std::vector<int> indices (1);
	std::vector<float> distances (1);

	// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
	pcl::PointXYZ picked_pt;
	event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
	search.nearestKSearch (picked_pt, 1, indices, distances);
	picked_points.push_back(picked_pt);
}

void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {

    // Instantiate various clouds
    pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
    pcl::PointCloud<pcl::PointXYZ> cloud;

    // Convert to PCL data type
    pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);

    // Apply Voxel Filter on PCLPointCloud2
    vox.setInputCloud (cloudPtr);
    vox.setInputCloud (cloudPtr);
    vox.filter (*cloud_intermediate);

    // Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
    pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();

    // Apply Passthrough Filter
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.3, 1);
    pass.setInputCloud (cloud_p);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    // Apply Passthrough Filter
    pass.setFilterFieldName ("x");
    pass.setFilterLimits (-0.2, 0.2);
    pass.setInputCloud (cloud_p);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.0, 3.0);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    // Apply Statistical Noise Filter
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);

    // Planar segmentation: Remove large planes? Or extract floor?
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    int nr_points = (int) cloud_p->points.size ();

    Eigen::Vector3f lol (0, 1, 0);
    seg.setEpsAngle(  30.0f * (3.14f/180.0f) );
    seg.setAxis(lol);
    //while(cloud_p->points.size () > 0.2 * nr_points) {
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
    seg.setInputCloud (cloud_p);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0)
    {
        //break;
    }
    else {
        /*std::cout << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << "\n";*/
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_p);
        extract.setIndices(inliers);
        extract.setNegative(true);
        extract.filter(*cloud_p);
    }
    //}

    Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
    seg.setAxis(lol_p);
    while(cloud_p->points.size () > 0.1 * nr_points) {

        seg.setInputCloud (cloud_p);
        seg.segment (*inliers, *coefficients);

        if (inliers->indices.size () == 0)
        {
            break;
        }
        else {
            /*std::cout << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << "\n";*/
            pcl::ExtractIndices<pcl::PointXYZ> extract;
            extract.setInputCloud(cloud_p);
            extract.setIndices(inliers);
            extract.setNegative(true);
            extract.filter(*cloud_p);
//.........这里部分代码省略.........

/* ---[ */
int
main (int argc, char** argv)
{
  if (argc < 2)
  {
    std::cout << "need path to table_scene_mug_stereo_textured.pcd file\n";
    return (-1);
  }

  pcl::io::loadPCDFile (argv [1], *organized_sparse_cloud);
  
  // create unorganized cloud
  unorganized_dense_cloud->resize (unorganized_point_count);
  unorganized_dense_cloud->height = 1;
  unorganized_dense_cloud->width = unorganized_point_count;
  unorganized_dense_cloud->is_dense = true;
  
  unorganized_sparse_cloud->resize (unorganized_point_count);
  unorganized_sparse_cloud->height = 1;
  unorganized_sparse_cloud->width = unorganized_point_count;
  unorganized_sparse_cloud->is_dense = false;

  PointXYZ point;
  for (unsigned pIdx = 0; pIdx < unorganized_point_count; ++pIdx)
  {
    point.x = rand_float ();
    point.y = rand_float ();
    point.z = rand_float ();

    unorganized_dense_cloud->points [pIdx] = point;
    
    if (rand_uint () == 0)
      unorganized_sparse_cloud->points [pIdx].x = unorganized_sparse_cloud->points [pIdx].y = unorganized_sparse_cloud->points [pIdx].z = std::numeric_limits<float>::quiet_NaN ();
    else
      unorganized_sparse_cloud->points [pIdx] = point;
  }
  
  unorganized_grid_cloud->reserve (1000);
  unorganized_grid_cloud->height = 1;
  unorganized_grid_cloud->width = 1000;
  unorganized_grid_cloud->is_dense = true;
  
  // values between 0 and 1
  for (unsigned xIdx = 0; xIdx < 10; ++xIdx)
  {
    for (unsigned yIdx = 0; yIdx < 10; ++yIdx)
    {
      for (unsigned zIdx = 0; zIdx < 10; ++zIdx)
      {
        point.x = 0.1f * static_cast<float>(xIdx);
        point.y = 0.1f * static_cast<float>(yIdx);
        point.z = 0.1f * static_cast<float>(zIdx);
        unorganized_grid_cloud->push_back (point);
      }
    }
  }
  
  createIndices (organized_input_indices, static_cast<unsigned> (organized_sparse_cloud->size () - 1));
  createIndices (unorganized_input_indices, unorganized_point_count - 1);
  
  brute_force.setSortedResults (true);
  KDTree.setSortedResults (true);
  octree_search.setSortedResults (true);
  organized.setSortedResults (true);
  
  unorganized_search_methods.push_back (&brute_force);
  unorganized_search_methods.push_back (&KDTree);
  unorganized_search_methods.push_back (&octree_search);
  
  organized_search_methods.push_back (&brute_force);
  organized_search_methods.push_back (&KDTree);
  organized_search_methods.push_back (&octree_search);
  organized_search_methods.push_back (&organized);
  
  createQueryIndices (unorganized_dense_cloud_query_indices, unorganized_dense_cloud, query_count);
  createQueryIndices (unorganized_sparse_cloud_query_indices, unorganized_sparse_cloud, query_count);
  createQueryIndices (organized_sparse_query_indices, organized_sparse_cloud, query_count);
  
  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}