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));
}
OctreePointCloudSearch<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_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
// instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
OctreePointCloudSearch<PointXYZ> octreeB (resolution);
OctreePointCloudPointVector<PointXYZ> octreeC (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octreeA.setInputCloud (cloudA);
octreeB.setInputCloud (cloudB);
octreeC.setInputCloud (cloudB);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloudA->points.clear ();
octreeA.deleteTree ();
cloudB->points.clear ();
octreeB.deleteTree ();
octreeC.deleteTree ();
for (point = 0; point < pointcount; point++)
{
// gereate a random point
PointXYZ newPoint (1024.0 * (float)rand () / (float)RAND_MAX, 1024.0 * (float)rand () / (float)RAND_MAX,
1024.0 * (float)rand () / (float)RAND_MAX);
if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
{
// add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
octreeA.addPointToCloud (newPoint, cloudA);
}
// OctreePointCloudPointVector can store all points..
cloudB->points.push_back (newPoint);
}
ASSERT_EQ (octreeA.getLeafCount(), cloudA->points.size());
// checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
for (i = 0; i < cloudA->points.size (); i++)
{
ASSERT_EQ (octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true);
octreeA.deleteVoxelAtPoint(cloudA->points[i]);
ASSERT_EQ (octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false);
}
ASSERT_EQ (octreeA.getLeafCount(), 0);
// check if all points from leaf data can be found in input pointcloud data sets
octreeB.defineBoundingBox();
octreeB.addPointsFromInputCloud();
for (i = 0; i < cloudB->points.size (); i++)
{
std::vector<int> pointIdxVec;
octreeB.voxelSearch(cloudB->points[i], pointIdxVec);
bool bIdxFound = false;
std::vector<int>::const_iterator current = pointIdxVec.begin();
while (current != pointIdxVec.end())
{
if (*current == (int)i)
{
bIdxFound = true;
break;
}
++current;
}
ASSERT_EQ (bIdxFound, true);
}
// test octree pointcloud serialization
std::vector<char> treeBinaryB;
std::vector<char> treeBinaryC;
std::vector<int> leafVectorB;
std::vector<int> leafVectorC;
double minX, minY, minZ, maxX, maxY, maxZ;
// serialize octreeB
octreeB.serializeTree(treeBinaryB, leafVectorB);
octreeB.getBoundingBox ( minX, minY,minZ, maxX, maxY, maxZ );
ASSERT_EQ (cloudB->points.size(), leafVectorB.size());
// deserialize into octreeC
octreeC.deleteTree();
octreeC.defineBoundingBox ( minX, minY,minZ, maxX, maxY, maxZ );
octreeC.deserializeTree(treeBinaryB, leafVectorB);
// retrieve point data content of octreeC
octreeC.serializeTree(treeBinaryC, leafVectorC);
// check if data is consistent
ASSERT_EQ (octreeB.getLeafCount(), octreeC.getLeafCount());
ASSERT_EQ (treeBinaryB.size(), treeBinaryC.size() );
ASSERT_EQ (octreeB.getLeafCount(), cloudB->points.size());
for (i = 0; i < leafVectorB.size(); ++i)
{
ASSERT_EQ (leafVectorB[i], leafVectorC[i]);
}
// check if binary octree output of both trees is equal
for (i = 0; i < treeBinaryB.size(); ++i)
{
ASSERT_EQ (treeBinaryB[i], treeBinaryC[i]);
}
}
}
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
OctreePointCloudSearch<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
OctreePointCloudSearch<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_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
// instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
OctreePointCloudSearch<PointXYZ> octreeB (resolution);
OctreePointCloudPointVector<PointXYZ> octreeC (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octreeA.setInputCloud (cloudA);
octreeB.setInputCloud (cloudB);
octreeC.setInputCloud (cloudB);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloudA->points.clear ();
octreeA.deleteTree ();
cloudB->points.clear ();
octreeB.deleteTree ();
octreeC.deleteTree ();
for (point = 0; point < pointcount; point++)
{
// gereate a random point
PointXYZ newPoint (1024.0 * (float)rand () / (float)RAND_MAX, 1024.0 * (float)rand () / (float)RAND_MAX,
1024.0 * (float)rand () / (float)RAND_MAX);
if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
{
// add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
octreeA.addPointToCloud (newPoint, cloudA);
}
// OctreePointCloudPointVector can store all points..
cloudB->points.push_back (newPoint);
}
ASSERT_EQ(octreeA.getLeafCount(), cloudA->points.size());
// checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
for (i = 0; i < cloudA->points.size (); i++)
{
ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true);
octreeA.deleteVoxelAtPoint (cloudA->points[i]);
ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false);
}
ASSERT_EQ(octreeA.getLeafCount(), (unsigned int)0);
// check if all points from leaf data can be found in input pointcloud data sets
octreeB.defineBoundingBox ();
octreeB.addPointsFromInputCloud ();
// test iterator
OctreePointCloudSearch<PointXYZ>::Iterator b_it (octreeB);
unsigned int node_count = 0;
unsigned int branch_count = 0;
unsigned int leaf_count = 0;
double minx, miny, minz, maxx, maxy, maxz;
octreeB.getBoundingBox (minx, miny, minz, maxx, maxy, maxz);
// iterate over tree
while (*++b_it)
{
// depth should always be less than tree depth
unsigned int depth = b_it.getCurrentOctreeDepth ();
ASSERT_LE(depth, octreeB.getTreeDepth());
Eigen::Vector3f voxel_min, voxel_max;
octreeB.getVoxelBounds (b_it, voxel_min, voxel_max);
ASSERT_GE(voxel_min.x (), minx - 1e-4);
ASSERT_GE(voxel_min.y (), miny - 1e-4);
ASSERT_GE(voxel_min.z (), minz - 1e-4);
ASSERT_LE(voxel_max.x (), maxx + 1e-4);
ASSERT_LE(voxel_max.y (), maxy + 1e-4);
ASSERT_LE(voxel_max.z (), maxz + 1e-4);
// store node, branch and leaf count
const OctreeNode* node = b_it.getCurrentOctreeNode ();
if (node->getNodeType () == BRANCH_NODE)
{
branch_count++;
}
else if (node->getNodeType () == LEAF_NODE)
{
leaf_count++;
}
node_count++;
}
// compare node, branch and leaf count against actual tree values
ASSERT_EQ(node_count, branch_count + leaf_count);
ASSERT_EQ(leaf_count, octreeB.getLeafCount ());
for (i = 0; i < cloudB->points.size (); i++)
{
std::vector<int> pointIdxVec;
octreeB.voxelSearch (cloudB->points[i], pointIdxVec);
bool bIdxFound = false;
std::vector<int>::const_iterator current = pointIdxVec.begin ();
while (current != pointIdxVec.end ())
{
if (*current == (int)i)
{
bIdxFound = true;
break;
}
++current;
}
ASSERT_EQ(bIdxFound, true);
}
// test octree pointcloud serialization
std::vector<char> treeBinaryB;
std::vector<char> treeBinaryC;
std::vector<int> leafVectorB;
std::vector<int> leafVectorC;
double minX, minY, minZ, maxX, maxY, maxZ;
// serialize octreeB
octreeB.serializeTree (treeBinaryB, leafVectorB);
octreeB.getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
ASSERT_EQ(cloudB->points.size(), leafVectorB.size());
// deserialize into octreeC
octreeC.deleteTree ();
octreeC.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
octreeC.deserializeTree (treeBinaryB, leafVectorB);
// retrieve point data content of octreeC
octreeC.serializeTree (treeBinaryC, leafVectorC);
// check if data is consistent
ASSERT_EQ(octreeB.getLeafCount(), octreeC.getLeafCount());
ASSERT_EQ(treeBinaryB.size(), treeBinaryC.size());
ASSERT_EQ(octreeB.getLeafCount(), cloudB->points.size());
for (i = 0; i < leafVectorB.size (); ++i)
{
ASSERT_EQ(leafVectorB[i], leafVectorC[i]);
}
// check if binary octree output of both trees is equal
for (i = 0; i < treeBinaryB.size (); ++i)
{
ASSERT_EQ(treeBinaryB[i], treeBinaryC[i]);
}
}
}
TEST (PCL, Octree_Pointcloud_Box_Search)
{
const unsigned int test_runs = 30;
unsigned int test_id;
// instantiate point cloud
PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
size_t i;
srand (time (NULL));
// create octree
OctreePointCloudSearch<PointXYZ> octree (1);
octree.setInputCloud (cloudIn);
for (test_id = 0; test_id < test_runs; test_id++)
{
std::vector<int> k_indices;
// generate point cloud
cloudIn->width = 300;
cloudIn->height = 1;
cloudIn->points.resize (cloudIn->width * cloudIn->height);
for (i = 0; i < cloudIn->points.size(); i++)
{
cloudIn->points[i] = PointXYZ (10.0 * ((double)rand () / (double)RAND_MAX),
10.0 * ((double)rand () / (double)RAND_MAX),
10.0 * ((double)rand () / (double)RAND_MAX));
}
// octree points to octree
octree.deleteTree ();
octree.addPointsFromInputCloud ();
// define a random search area
Eigen::Vector3f lowerBoxCorner (4.0 * ((double)rand () / (double)RAND_MAX),
4.0 * ((double)rand () / (double)RAND_MAX),
4.0 * ((double)rand () / (double)RAND_MAX));
Eigen::Vector3f upperBoxCorner (5 + 4.0 * ((double)rand () / (double)RAND_MAX),
5 + 4.0 * ((double)rand () / (double)RAND_MAX),
5 + 4.0 * ((double)rand () / (double)RAND_MAX));
octree.boxSearch (lowerBoxCorner, upperBoxCorner, k_indices);
// test every point in point cloud
for (i = 0; i < 300; i++)
{
std::size_t j;
bool inBox;
bool idxInResults;
const PointXYZ& pt = cloudIn->points[i];
inBox = (pt.x > lowerBoxCorner (0)) && (pt.x < upperBoxCorner (0)) &&
(pt.y > lowerBoxCorner (1)) && (pt.y < upperBoxCorner (1)) &&
(pt.z > lowerBoxCorner (2)) && (pt.z < upperBoxCorner (2));
idxInResults = false;
for (j = 0; (j < k_indices.size ()) && (!idxInResults); ++j)
{
if (i == (unsigned int)k_indices[j])
idxInResults = true;
}
ASSERT_EQ(idxInResults, inBox);
}
}
}
TEST (PCL, Octree_Pointcloud_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
// instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
OctreePointCloudSearch<PointXYZ> octreeB (resolution);
OctreePointCloudPointVector<PointXYZ> octreeC (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octreeA.setInputCloud (cloudA);
octreeB.setInputCloud (cloudB);
octreeC.setInputCloud (cloudB);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloudA->points.clear ();
octreeA.deleteTree ();
cloudB->points.clear ();
octreeB.deleteTree ();
octreeC.deleteTree ();
for (point = 0; point < pointcount; point++)
{
// gereate a random point
PointXYZ newPoint (static_cast<float> (1024.0 * rand () / RAND_MAX),
static_cast<float> (1024.0 * rand () / RAND_MAX),
static_cast<float> (1024.0 * rand () / RAND_MAX));
if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
{
// add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
octreeA.addPointToCloud (newPoint, cloudA);
}
// OctreePointCloudPointVector can store all points..
cloudB->push_back (newPoint);
}
ASSERT_EQ (cloudA->points.size (), octreeA.getLeafCount ());
// checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
for (i = 0; i < cloudA->points.size (); i++)
{
ASSERT_TRUE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i]));
octreeA.deleteVoxelAtPoint (cloudA->points[i]);
ASSERT_FALSE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i]));
}
ASSERT_EQ (static_cast<unsigned int> (0), octreeA.getLeafCount ());
// check if all points from leaf data can be found in input pointcloud data sets
octreeB.defineBoundingBox ();
octreeB.addPointsFromInputCloud ();
// test iterator
OctreePointCloudPointVector<PointXYZ>::Iterator b_it;
OctreePointCloudPointVector<PointXYZ>::Iterator b_it_end = octreeB.end();
// iterate over tree
unsigned int node_count = 0;
unsigned int branch_count = 0;
unsigned int leaf_count = 0;
double minx, miny, minz, maxx, maxy, maxz;
octreeB.getBoundingBox (minx, miny, minz, maxx, maxy, maxz);
// iterate over tree
for (b_it = octreeB.begin(); b_it != b_it_end; ++b_it)
{
// depth should always be less than tree depth
unsigned int depth = b_it.getCurrentOctreeDepth ();
ASSERT_LE (depth, octreeB.getTreeDepth ());
Eigen::Vector3f voxel_min, voxel_max;
octreeB.getVoxelBounds (b_it, voxel_min, voxel_max);
ASSERT_GE (voxel_min.x (), minx - 1e-4);
ASSERT_GE (voxel_min.y (), miny - 1e-4);
ASSERT_GE (voxel_min.z (), minz - 1e-4);
ASSERT_LE (voxel_max.x (), maxx + 1e-4);
ASSERT_LE (voxel_max.y (), maxy + 1e-4);
ASSERT_LE (voxel_max.z (), maxz + 1e-4);
// store node, branch and leaf count
const OctreeNode* node = b_it.getCurrentOctreeNode ();
if (node->getNodeType () == BRANCH_NODE)
{
branch_count++;
}
else if (node->getNodeType () == LEAF_NODE)
{
leaf_count++;
}
node_count++;
}
// compare node, branch and leaf count against actual tree values
ASSERT_EQ (branch_count + leaf_count, node_count);
ASSERT_EQ (octreeB.getLeafCount (), leaf_count);
for (i = 0; i < cloudB->points.size (); i++)
{
std::vector<int> pointIdxVec;
octreeB.voxelSearch (cloudB->points[i], pointIdxVec);
bool bIdxFound = false;
std::vector<int>::const_iterator current = pointIdxVec.begin ();
while (current != pointIdxVec.end ())
{
if (*current == static_cast<int> (i))
{
bIdxFound = true;
break;
}
++current;
}
ASSERT_TRUE (bIdxFound);
}
}
}
