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);
}
}
}
