C++ KdTreePtr类代码示例

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

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

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

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
  // Check if normals have to be computed/saved
  if (compute_normals_)
  {
    normals_.reset (new NormalCloud);
    // Copy the header
    normals_->header = input_->header;
    // Clear the fields in case the method exits before computation
    normals_->width = normals_->height = 0;
    normals_->points.clear ();
  }


  // Copy the header
  output.header = input_->header;
  output.width = output.height = 0;
  output.points.clear ();

  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
  {
    PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
    return;
  }

  if (!initCompute ())
    return;


  // Initialize the spatial locator
  if (!tree_)
  {
    KdTreePtr tree;
    if (input_->isOrganized ())
      tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
    else
      tree.reset (new pcl::search::KdTree<PointInT> (false));
    setSearchMethod (tree);
  }

  // Send the surface dataset to the spatial locator
  tree_->setInputCloud (input_, indices_);

  // Initialize random number generator if necessary
  switch (upsample_method_)
  {
    case (RANDOM_UNIFORM_DENSITY):
    {
      boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0)));
      float tmp = static_cast<float> (search_radius_ / 2.0f);
      boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp);
      rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib);
      break;
    }
    case (VOXEL_GRID_DILATION):
    {
      mls_results_.resize (input_->size ());
      break;
    }
    default:
      break;
  }

  // Perform the actual surface reconstruction
  performProcessing (output);

  if (compute_normals_)
  {
    normals_->height = 1;
    normals_->width = static_cast<uint32_t> (normals_->size ());

    // TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point
    for (unsigned int i = 0; i < output.size (); ++i)
    {
      typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
    }

  }

  // Set proper widths and heights for the clouds
  output.height = 1;
  output.width = static_cast<uint32_t> (output.size ());

  deinitCompute ();
}

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

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
  // Reset or initialize the collection of indices
  corresponding_input_indices_.reset (new PointIndices);

  // Check if normals have to be computed/saved
  if (compute_normals_)
  {
    normals_.reset (new NormalCloud);
    // Copy the header
    normals_->header = input_->header;
    // Clear the fields in case the method exits before computation
    normals_->width = normals_->height = 0;
    normals_->points.clear ();
  }

  // Copy the header
  output.header = input_->header;
  output.width = output.height = 0;
  output.points.clear ();

  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
  {
    PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
    return;
  }

  // Check if distinct_cloud_ was set
  if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
  {
    PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
    return;
  }

  if (!initCompute ())
    return;

  // Initialize the spatial locator
  if (!tree_)
  {
    KdTreePtr tree;
    if (input_->isOrganized ())
      tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
    else
      tree.reset (new pcl::search::KdTree<PointInT> (false));
    setSearchMethod (tree);
  }

  // Send the surface dataset to the spatial locator
  tree_->setInputCloud (input_);

  switch (upsample_method_)
  {
    // Initialize random number generator if necessary
    case (RANDOM_UNIFORM_DENSITY):
    {
      rng_alg_.seed (static_cast<unsigned> (std::time (0)));
      float tmp = static_cast<float> (search_radius_ / 2.0f);
      boost::uniform_real<float> uniform_distrib (-tmp, tmp);
      rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));

      break;
    }
    case (VOXEL_GRID_DILATION):
    case (DISTINCT_CLOUD):
    {
      if (!cache_mls_results_)
        PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");

      cache_mls_results_ = true;
      break;
    }
    default:
      break;
  }

  if (cache_mls_results_)
  {
    mls_results_.resize (input_->size ());
  }
  else
  {
    mls_results_.resize (1); // Need to have a reference to a single dummy result.
  }

  // Perform the actual surface reconstruction
  performProcessing (output);

  if (compute_normals_)
  {
    normals_->height = 1;
    normals_->width = static_cast<uint32_t> (normals_->size ());

    for (unsigned int i = 0; i < output.size (); ++i)
    {
      typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
//.........这里部分代码省略.........

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

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

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

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

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