TEST (PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search)
{
const unsigned int test_runs = 100;
unsigned int test_id;
unsigned int bestMatchCount = 0;
// instantiate point cloud
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
size_t i;
srand (time (NULL));
double voxelResolution = 0.1;
// create octree
OctreePointCloud<PointXYZ> octree (voxelResolution);
octree.setInputCloud (cloudIn);
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));
// 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));
}
// brute force search
double pointDist;
double BFdistance = numeric_limits<double>::max ();
int BFindex = 0;
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));
if (pointDist<BFdistance)
{
BFindex = i;
BFdistance = pointDist;
}
}
int ANNindex;
float ANNdistance;
// octree approx. nearest neighbor search
octree.deleteTree();
octree.addPointsFromInputCloud();
octree.approxNearestSearch (searchPoint, ANNindex, ANNdistance);
if (BFindex==ANNindex)
{
EXPECT_NEAR (ANNdistance, BFdistance, 1e-4);
bestMatchCount++;
}
}
// we should have found the absolute nearest neighbor at least once
ASSERT_EQ ( (bestMatchCount > 0) , true);
}
