TEST (PCL, Octree_Pointcloud_Bounds)
{
const double SOME_RESOLUTION (10 + 1/3.0);
const int SOME_DEPTH (4);
const double DESIRED_MAX = ((1<<SOME_DEPTH) + 0.5)*SOME_RESOLUTION;
const double DESIRED_MIN = 0;
OctreePointCloud<PointXYZ> tree (SOME_RESOLUTION);
tree.defineBoundingBox (DESIRED_MIN, DESIRED_MIN, DESIRED_MIN, DESIRED_MAX, DESIRED_MAX, DESIRED_MAX);
double min_x, min_y, min_z, max_x, max_y, max_z;
tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
ASSERT_GE (max_x, DESIRED_MAX);
ASSERT_GE (DESIRED_MIN, min_x);
const double LARGE_MIN = 1e7-45*SOME_RESOLUTION;
const double LARGE_MAX = 1e7-5*SOME_RESOLUTION;
tree.defineBoundingBox (LARGE_MIN, LARGE_MIN, LARGE_MIN, LARGE_MAX, LARGE_MAX, LARGE_MAX);
tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
const unsigned int depth = tree.getTreeDepth ();
tree.defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
ASSERT_EQ (depth, tree.getTreeDepth ());
double min_x2, min_y2, min_z2, max_x2, max_y2, max_z2;
tree.getBoundingBox (min_x2, min_y2, min_z2, max_x2, max_y2, max_z2);
ASSERT_DOUBLE_EQ (min_x2, min_x);
ASSERT_DOUBLE_EQ (max_x2, max_x);
}
TEST (PCL, Octree_Pointcloud_Iterator_Test)
{
// instantiate point cloud and fill it with point data
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
for (float z = 0.05f; z < 7.0f; z += 0.1f)
for (float y = 0.05f; y < 7.0f; y += 0.1f)
for (float x = 0.05f; x < 7.0f; x += 0.1f)
cloudIn->points.push_back (PointXYZ (x, y, z));
cloudIn->width = cloudIn->points.size ();
cloudIn->height = 1;
OctreePointCloud<PointXYZ> octreeA (1.0f); // low resolution
// add point data to octree
octreeA.setInputCloud (cloudIn);
octreeA.addPointsFromInputCloud ();
// instantiate iterator for octreeA
OctreePointCloud<PointXYZ>::LeafNodeIterator it1 (octreeA);
std::vector<int> indexVector;
unsigned int leafNodeCounter = 0;
// test preincrement
++it1;
it1.getData (indexVector);
leafNodeCounter++;
// test postincrement
it1++;
it1.getData (indexVector);
leafNodeCounter++;
while (*++it1)
{
it1.getData (indexVector);
leafNodeCounter++;
}
ASSERT_EQ (indexVector.size(), cloudIn->points.size () );
ASSERT_EQ (leafNodeCounter, octreeA.getLeafCount() );
OctreePointCloud<PointXYZ>::Iterator it2 (octreeA);
unsigned int traversCounter = 0;
while ( *++it2 )
{
traversCounter++;
}
ASSERT_EQ (traversCounter > octreeA.getLeafCount() + octreeA.getBranchCount() , true );
}
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);
}
}
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);
}
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();
}
}
}
TEST (PCL, Octree_Pointcloud_Iterator_Test)
{
// instantiate point cloud and fill it with point data
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
for (float z = 0.05f; z < 7.0f; z += 0.1f)
for (float y = 0.05f; y < 7.0f; y += 0.1f)
for (float x = 0.05f; x < 7.0f; x += 0.1f)
cloudIn->points.push_back (PointXYZ (x, y, z));
cloudIn->width = cloudIn->points.size ();
cloudIn->height = 1;
OctreePointCloud<PointXYZ> octreeA (1.0f); // low resolution
// add point data to octree
octreeA.setInputCloud (cloudIn);
octreeA.addPointsFromInputCloud ();
// instantiate iterator for octreeA
OctreePointCloud<PointXYZ>::LeafNodeIterator it1 (octreeA);
std::vector<int> indexVector;
unsigned int leafNodeCounter = 0;
// test preincrement
++it1;
it1.getData (indexVector);
leafNodeCounter++;
// test postincrement
it1++;
it1.getData (indexVector);
leafNodeCounter++;
while (*++it1)
{
it1.getData (indexVector);
leafNodeCounter++;
}
ASSERT_EQ(indexVector.size(), cloudIn->points.size ());
ASSERT_EQ(leafNodeCounter, octreeA.getLeafCount());
OctreePointCloud<PointXYZ>::Iterator it2 (octreeA);
unsigned int traversCounter = 0;
while (*++it2)
{
traversCounter++;
}
ASSERT_EQ(traversCounter > octreeA.getLeafCount() + octreeA.getBranchCount(), true);
// breadth-first iterator test
OctreePointCloud<PointXYZ>::BreadthFirstIterator bfIt (octreeA);
unsigned int lastDepth = 0;
unsigned int branchNodeCount = 1;
unsigned int leafNodeCount = 0;
bool leafNodeVisited = false;
while (*++bfIt)
{
// tree depth of visited nodes must grow
ASSERT_EQ( bfIt.getCurrentOctreeDepth()>=lastDepth, true);
lastDepth = bfIt.getCurrentOctreeDepth ();
if (bfIt.isBranchNode ())
{
branchNodeCount++;
// leaf nodes are traversed in the end
ASSERT_EQ( leafNodeVisited, false);
}
if (bfIt.isLeafNode ())
{
leafNodeCount++;
leafNodeVisited = true;
}
}
// check if every branch node and every leaf node has been visited
ASSERT_EQ( leafNodeCount, octreeA.getLeafCount());
ASSERT_EQ( branchNodeCount, octreeA.getBranchCount());
}
TEST (PCL, Octree_Pointcloud_Iterator_Test)
{
// instantiate point cloud and fill it with point data
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
for (float z = 0.05f; z < 7.0f; z += 0.1f)
for (float y = 0.05f; y < 7.0f; y += 0.1f)
for (float x = 0.05f; x < 7.0f; x += 0.1f)
cloudIn->push_back (PointXYZ (x, y, z));
cloudIn->width = static_cast<uint32_t> (cloudIn->points.size ());
cloudIn->height = 1;
OctreePointCloud<PointXYZ> octreeA (1.0f); // low resolution
// add point data to octree
octreeA.setInputCloud (cloudIn);
octreeA.addPointsFromInputCloud ();
// instantiate iterator for octreeA
OctreePointCloud<PointXYZ>::LeafNodeDepthFirstIterator it1;
OctreePointCloud<PointXYZ>::LeafNodeDepthFirstIterator it1_end = octreeA.leaf_depth_end();
std::vector<int> indexVector;
unsigned int leafNodeCounter = 0;
for (it1 = octreeA.leaf_depth_begin(); it1 != it1_end; ++it1)
{
it1.getLeafContainer().getPointIndices(indexVector);
leafNodeCounter++;
}
ASSERT_EQ (cloudIn->points.size (), indexVector.size());
ASSERT_EQ (octreeA.getLeafCount (), leafNodeCounter);
OctreePointCloud<PointXYZ>::Iterator it2;
OctreePointCloud<PointXYZ>::Iterator it2_end = octreeA.end();
unsigned int traversCounter = 0;
for (it2 = octreeA.begin(); it2 != it2_end; ++it2)
{
traversCounter++;
}
ASSERT_EQ (octreeA.getLeafCount () + octreeA.getBranchCount (), traversCounter);
// breadth-first iterator test
unsigned int lastDepth = 0;
unsigned int branchNodeCount = 0;
unsigned int leafNodeCount = 0;
bool leafNodeVisited = false;
OctreePointCloud<PointXYZ>::BreadthFirstIterator bfIt;
const OctreePointCloud<PointXYZ>::BreadthFirstIterator bfIt_end = octreeA.breadth_end();
for (bfIt = octreeA.breadth_begin(); bfIt != bfIt_end; ++bfIt)
{
// tree depth of visited nodes must grow
ASSERT_TRUE (bfIt.getCurrentOctreeDepth () >= lastDepth);
lastDepth = bfIt.getCurrentOctreeDepth ();
if (bfIt.isBranchNode ())
{
branchNodeCount++;
// leaf nodes are traversed in the end
ASSERT_FALSE (leafNodeVisited);
}
if (bfIt.isLeafNode ())
{
leafNodeCount++;
leafNodeVisited = true;
}
}
// check if every branch node and every leaf node has been visited
ASSERT_EQ (octreeA.getLeafCount (), leafNodeCount);
ASSERT_EQ (octreeA.getBranchCount (), branchNodeCount);
}
