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

pcl::IndicesPtr normalFiltering(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float angleMax,
		const Eigen::Vector4f & normal,
		float radiusSearch,
		const Eigen::Vector4f & viewpoint)
{
	typedef typename pcl::search::KdTree<PointT> KdTree;
	typedef typename KdTree::Ptr KdTreePtr;

	pcl::NormalEstimation<PointT, pcl::Normal> ne;
	ne.setInputCloud (cloud);
	if(indices->size())
	{
		ne.setIndices(indices);
	}

	KdTreePtr tree (new KdTree(false));

	if(indices->size())
	{
		tree->setInputCloud(cloud, indices);
	}
	else
	{
		tree->setInputCloud(cloud);
	}
	ne.setSearchMethod (tree);

	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

	ne.setRadiusSearch (radiusSearch);
	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
	{
		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
	}

	ne.compute (*cloud_normals);

	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
	int oi = 0; // output iterator
	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
	for(unsigned int i=0; i<cloud_normals->size(); ++i)
	{
		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
		float angle = pcl::getAngle3D(normal, v);
		if(angle < angleMax)
		{
			output->at(oi++) = indices->size()!=0?indices->at(i):i;
		}
	}
	output->resize(oi);

	return output;
}

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

  if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
  {
    std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  indices.resize (cloud.points.size ());
  for (int i = 0; i < static_cast<int> (indices.size ()); ++i)
    indices[i] = i;

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

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

pcl::IndicesPtr radiusFiltering(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float radiusSearch,
		int minNeighborsInRadius)
{
	typedef typename pcl::search::KdTree<PointT> KdTree;
	typedef typename KdTree::Ptr KdTreePtr;
	KdTreePtr tree (new KdTree(false));

	if(indices->size())
	{
		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
		int oi = 0; // output iterator
		tree->setInputCloud(cloud, indices);
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			std::vector<int> kIndices;
			std::vector<float> kDistances;
			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
			if(k > minNeighborsInRadius)
			{
				output->at(oi++) = indices->at(i);
			}
		}
		output->resize(oi);
		return output;
	}
	else
	{
		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
		int oi = 0; // output iterator
		tree->setInputCloud(cloud);
		for(unsigned int i=0; i<cloud->size(); ++i)
		{
			std::vector<int> kIndices;
			std::vector<float> kDistances;
			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
			if(k > minNeighborsInRadius)
			{
				output->at(oi++) = i;
			}
		}
		output->resize(oi);
		return output;
	}
}

bool fpfh_cb(feature_extractor_fpfh::FPFHCalc::Request &req,
             feature_extractor_fpfh::FPFHCalc::Response &res)
{
    float leaf_size = .01;
    pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
    sensor_msgs::PointCloud2::Ptr input_cloud(new sensor_msgs::PointCloud2());
    sensor_msgs::convertPointCloudToPointCloud2(req.input, *input_cloud);
    //sensor_msgs::PointCloud2::Ptr input_cloud(&req.input);

    sensor_msgs::PointCloud2::Ptr cloud_filtered(new sensor_msgs::PointCloud2());
    sor.setInputCloud(input_cloud);
    sor.setLeafSize (leaf_size, leaf_size, leaf_size);
    sor.filter(*cloud_filtered);
    ROS_INFO("after filtering: %d points", cloud_filtered->width * cloud_filtered->height);

    PointCloud<PointXYZ> cloud;
    fromROSMsg(*cloud_filtered, cloud);
    std::vector<int> indices;
    indices.resize (cloud.points.size());
    for (size_t i = 0; i < indices.size (); ++i) { indices[i] = i; }

    // make tree
    KdTreePtr tree;
    tree.reset(new KdTreeFLANN<PointXYZ> (false));
    tree->setInputCloud(cloud.makeShared());

    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
    boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices));
    NormalEstimation<PointXYZ, Normal> normal_estimator;

    // set normal estimation parameters
    normal_estimator.setIndices(indicesptr);
    normal_estimator.setSearchMethod(tree);
    normal_estimator.setKSearch(10); // Use 10 nearest neighbors to estimate the normals

    // estimate
    ROS_INFO("Calculating normals...\n");
    normal_estimator.setInputCloud(cloud.makeShared());
    normal_estimator.compute(*normals);

    // calculate FPFH
    //FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh;
    FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33> fpfh(4);    // instantiate 4 threads

    // object
    PointCloud<FPFHSignature33>::Ptr fphists (new PointCloud<FPFHSignature33>());

    // set parameters
    int d1, d2, d3;
    d1 = d2 = d3 = 11;
    fpfh.setNrSubdivisions(d1, d2, d3);
    fpfh.setIndices(indicesptr);
    fpfh.setSearchMethod(tree);
    fpfh.setKSearch(50);

    // estimate
    ROS_INFO("Calculating fpfh...\n");
    fpfh.setInputNormals(normals);
    fpfh.setInputCloud(cloud.makeShared());
    fpfh.compute(*fphists);

    res.hist.dim[0] = d1;
    res.hist.dim[1] = d2;
    res.hist.dim[2] = d3;
    unsigned int total_size = d1+d2+d3;
    res.hist.histograms.resize(total_size * cloud.points.size());
    res.hist.points3d.resize(3*cloud.points.size());

    ROS_INFO("copying into message...\n");
    for (unsigned int i = 0; i < cloud.points.size(); i++)
    {
        for (unsigned int j = 0; j < total_size; j++)
        {
            res.hist.histograms[i*total_size + j] = fphists->points[i].histogram[j];
            //if (i == 0)
            //{
            //    printf(">> %.2f \n", fphists->points[i].histogram[j]);
            //}

            //if (i == 4)
            //{
            //    printf("X %.2f \n", fphists->points[i].histogram[j]);
            //}
        }

        res.hist.points3d[3*i]   = cloud.points[i].x;
        res.hist.points3d[3*i+1] = cloud.points[i].y;
        res.hist.points3d[3*i+2] = cloud.points[i].z;
        //if (i == 0)
        //    printf(">> 0  %.4f %.4f %.4f \n", cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
        //if (i == 4)
        //    printf(">> 4  %.4f %.4f %.4f \n", cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
    }

    res.hist.npoints = cloud.points.size();
    ROS_INFO("done.\n");
    //printf("%d\n", );
    // sensor_msgs::PointCloud2 req
    // new feature_extractor::FPFHist()
    return true;
//.........这里部分代码省略.........

/* ---[ */
int
main (int argc, char** argv)
{
  if (argc < 3)
  {
    std::cerr << "No test file given. Please download `bun0.pcd` and `milk.pcd` pass its path to the test." << std::endl;
    return (-1);
  }

  if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
  {
    std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  CloudPtr milk_loaded(new PointCloud<PointXYZ>());
  if (loadPCDFile<PointXYZ> (argv[2], *milk_loaded) < 0)
  {
    std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  indices.resize (cloud.points.size ());
  for (size_t i = 0; i < indices.size (); ++i)
  {
    indices[i] = static_cast<int>(i);
  }

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

  cloud_milk.reset(new PointCloud<PointXYZ>());
  CloudPtr grid;
  pcl::VoxelGrid < pcl::PointXYZ > grid_;
  grid_.setInputCloud (milk_loaded);
  grid_.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
  grid_.filter (*cloud_milk);

  tree_milk.reset (new search::KdTree<PointXYZ> (false));
  tree_milk->setInputCloud (cloud_milk);

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

	static void computeNormals(PointCloudPtr pcl_cloud_input, pcl::PointCloud<Normal>::Ptr pcl_cloud_normals, const double search_radius)
	{
		PointCloud mls_points;

		typedef pcl::search::KdTree<PointT> KdTree;
		typedef typename KdTree::Ptr KdTreePtr;

		// Create a KD-Tree
		KdTreePtr tree = boost::make_shared<pcl::search::KdTree<PointT> >();
		MovingLeastSquares<PointT, Normal> mls;

		tree->setInputCloud(pcl_cloud_input);
		mls.setOutputNormals(pcl_cloud_normals);
		mls.setInputCloud(pcl_cloud_input);
		//mls.setPolynomialFit(true);
		mls.setSearchMethod(tree);
		mls.setSearchRadius(search_radius);
		mls.reconstruct(mls_points);
	}

/* ---[ */
int
main (int argc, char** argv)
{
  if (argc < 4)
  {
    std::cerr << "No test file given. Please download `bun0.pcd` `bun03.pcd` `milk.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  //
  // Load first cloud and prepare objets to test 
  //
  if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
  {
    std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  cloud_for_lrf = cloud;

  indices.resize (cloud.size (), 0);
  indices_for_lrf.resize (cloud.size (), 0);
  for (size_t i = 0; i < indices.size (); ++i)
  {
    indices[i] = static_cast<int> (i);
    indices_for_lrf[i] = static_cast<int> (i);
  }

  tree.reset (new search::KdTree<PointXYZ> ());
  tree->setInputCloud (cloud.makeShared ());
  tree->setSortedResults (true);

  Eigen::Vector4f centroid;
  pcl::compute3DCentroid<PointXYZ, float> (cloud_for_lrf, centroid);

  Eigen::Vector4f max_pt;
  pcl::getMaxDistance<PointXYZ> (cloud_for_lrf, centroid, max_pt);
  radius_local_shot = (max_pt - centroid).norm();

  PointXYZ p_centroid;
  p_centroid.getVector4fMap () = centroid;
  cloud_for_lrf.push_back (p_centroid);
  PointNormal p_centroid_nr;
  p_centroid_nr.getVector4fMap() = centroid;
  ground_truth.push_back(p_centroid_nr);
  cloud_for_lrf.height = 1;
  cloud_for_lrf.width = cloud_for_lrf.size ();

  indices_for_lrf.push_back (cloud_for_lrf.width - 1);
  indices_local_shot.resize (1);
  indices_local_shot[0] = cloud_for_lrf.width - 1;

  //
  // Load second cloud and prepare objets to test 
  //
  if (loadPCDFile<PointXYZ> (argv[2], cloud2) < 0)
  {
    std::cerr << "Failed to read test file. Please download `bun03.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  indices2.resize (cloud2.size (), 0);
  for (size_t i = 0; i < indices2.size (); ++i)
    indices2[i] = static_cast<int> (i);

  tree2.reset (new search::KdTree<PointXYZ> ());
  tree2->setInputCloud (cloud2.makeShared ());
  tree2->setSortedResults (true);

  //
  // Load third cloud and prepare objets to test 
  //
  if (loadPCDFile<PointXYZ> (argv[3], cloud3) < 0)
  {
    std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  indices3.resize (cloud3.size (), 0);
  for (size_t i = 0; i < indices3.size (); ++i)
    indices3[i] = static_cast<int> (i);

  tree3.reset (new search::KdTree<PointXYZ> ());
  tree3->setInputCloud (cloud3.makeShared ());
  tree3->setSortedResults (true);

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