TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search)
{
const unsigned int test_runs = 1;
unsigned int test_id;
// instantiate point cloud
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
size_t i;
srand (time (NULL));
unsigned int K;
std::priority_queue<prioPointQueueEntry, std::vector<prioPointQueueEntry, Eigen::aligned_allocator<prioPointQueueEntry> > > pointCandidates;
// create octree
OctreePointCloud<PointXYZ> octree (0.1);
octree.setInputCloud (cloudIn);
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
std::vector<int> k_indices_bruteforce;
std::vector<float> k_sqr_distances_bruteforce;
for (test_id = 0; test_id < test_runs; test_id++)
{
// define a random search point
PointXYZ searchPoint (10.0 * ((double)rand () / (double)RAND_MAX), 10.0 * ((double)rand () / (double)RAND_MAX),
10.0 * ((double)rand () / (double)RAND_MAX));
K = rand () % 10;
// generate point cloud
cloudIn->width = 1000;
cloudIn->height = 1;
cloudIn->points.resize (cloudIn->width * cloudIn->height);
for (i = 0; i < 1000; i++)
{
cloudIn->points[i] = PointXYZ (5.0 * ((double)rand () / (double)RAND_MAX),
10.0 * ((double)rand () / (double)RAND_MAX),
10.0 * ((double)rand () / (double)RAND_MAX));
}
double pointDist;
k_indices.clear();
k_sqr_distances.clear();
k_indices_bruteforce.clear();
k_sqr_distances_bruteforce.clear();
// push all points and their distance to the search point into a priority queue - bruteforce approach.
for (i = 0; i < cloudIn->points.size (); i++)
{
pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
+ (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) + (cloudIn->points[i].z
- searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
prioPointQueueEntry pointEntry (cloudIn->points[i], pointDist, i);
pointCandidates.push (pointEntry);
}
// pop priority queue until we have the nearest K elements
while (pointCandidates.size () > K)
pointCandidates.pop ();
// copy results into vectors
while (pointCandidates.size ())
{
k_indices_bruteforce.push_back (pointCandidates.top ().pointIdx_);
k_sqr_distances_bruteforce.push_back (pointCandidates.top ().pointDistance_);
pointCandidates.pop ();
}
// octree nearest neighbor search
octree.deleteTree();
octree.addPointsFromInputCloud();
octree.nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
// compare nearest neighbor results of octree with bruteforce search
i = 0;
while (k_indices_bruteforce.size ())
{
ASSERT_EQ ( k_indices.back() , k_indices_bruteforce.back() );
EXPECT_NEAR (k_sqr_distances.back(), k_sqr_distances_bruteforce.back(), 1e-4);
k_indices_bruteforce.pop_back();
k_indices.pop_back();
k_sqr_distances_bruteforce.pop_back();
k_sqr_distances.pop_back();
}
}
}
