TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
{
const unsigned int test_runs = 100;
unsigned int test_id;
// instantiate point clouds
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudOut (new PointCloud<PointXYZ> ());
size_t i;
srand (time (NULL));
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));
cloudIn->width = 1000;
cloudIn->height = 1;
cloudIn->points.resize (cloudIn->width * cloudIn->height);
// generate point cloud data
for (i = 0; i < 1000; i++)
{
cloudIn->points[i] = PointXYZ (10.0 * ((double)rand () / (double)RAND_MAX),
10.0 * ((double)rand () / (double)RAND_MAX),
5.0 * ((double)rand () / (double)RAND_MAX));
}
OctreePointCloud<PointXYZ> octree (0.001);
// build octree
octree.setInputCloud (cloudIn);
octree.addPointsFromInputCloud();
double pointDist;
double searchRadius = 5.0 * ((double)rand () / (double)RAND_MAX);
// bruteforce radius search
vector<int> cloudSearchBruteforce;
for (i = 0; i < cloudIn->points.size (); i++)
{
pointDist = sqrt (
(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 <= searchRadius)
{
// add point candidates to vector list
cloudSearchBruteforce.push_back ((int)i);
}
}
vector<int> cloudNWRSearch;
vector<float> cloudNWRRadius;
// execute octree radius search
octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
ASSERT_EQ ( cloudNWRRadius.size() , cloudSearchBruteforce.size());
// check if result from octree radius search can be also found in bruteforce search
std::vector<int>::const_iterator current = cloudNWRSearch.begin();
while (current != cloudNWRSearch.end())
{
pointDist = sqrt (
(cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
(cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
(cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
);
ASSERT_EQ ( (pointDist<=searchRadius) , true);
++current;
}
// check if result limitation works
octree.radiusSearch(searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
ASSERT_EQ ( cloudNWRRadius.size() <= 5, true);
}
}
