Octree nearestKSearch: get coordinates of resulting points

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view

Octree nearestKSearch: get coordinates of resulting points

This post was updated on .
Hi there

I have a question concerning octrees. My octree is incrementally filled by adding pointclouds to the octree. After the octree is filled, I execute a nearestKSearch.
My problem is: How do I get the coordinate of the resulting points? The way it is done in the tutorials does not work in my case.
Below I added some minimal code to illustrate my problem.

pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr octree_frontiers_ptr_(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(0.2));

for(int i=0; i<5; i++)
   pcl::PointCloud<pcl::PointXYZ>::Ptr frontier_cloud_ptr_ (new pcl::PointCloud<pcl::PointXYZ>);
   // frontier_cloud_ptr_ is populated
   octree_frontiers_ptr_->setInputCloud (frontier_cloud_ptr_);
   octree_frontiers_ptr_->addPointsFromInputCloud ();

int K = 5;
pcl::PointXYZ searchPoint;
searchPoint.x = 0;
searchPoint.y = 0;
searchPoint.z = 0;

std::vector<int> pointIdxNKNSearch;
std::vector<float> pointNKNSquaredDistance;

if (octree_frontiers_ptr_->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    //in the example, the input cloud is used to get the coordinates. This is not possible in my case since I'm using multiple input clouds.
    // below the example of the tutorials (which does not work in my case)
    for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
        std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x
                        << "    " <<    cloud->points[ pointIdxNKNSearch[i] ].y
                        << "    " <<    cloud->points[ pointIdxNKNSearch[i] ].z
                        << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;

How can I access the coordinates of the resulting points without using the input point cloud?
Thanks for any help

PCL-users@pointclouds.org / http://pointclouds.org