C++ KdTree::setInputCloud方法代码示例

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

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