TEST (PCL, Octree_Dynamic_Depth_Test)
{
size_t i;
int test_runs = 100;
int pointcount = 300;
int test, point;
float resolution = 0.01f;
const static size_t leafAggSize = 5;
OctreePointCloudPointVector<PointXYZ> octree (resolution);
// create shared pointcloud instances
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
// assign input point clouds to octree
octree.setInputCloud (cloud);
for (test = 0; test < test_runs; ++test)
{
// clean up
cloud->points.clear ();
octree.deleteTree ();
PointXYZ newPoint (1.5, 2.5, 3.5);
cloud->push_back (newPoint);
for (point = 0; point < 15; point++)
{
// gereate a random point
PointXYZ newPoint (1.0, 2.0, 3.0);
// OctreePointCloudPointVector can store all points..
cloud->push_back (newPoint);
}
// check if all points from leaf data can be found in input pointcloud data sets
octree.defineBoundingBox ();
octree.enableDynamicDepth (leafAggSize);
octree.addPointsFromInputCloud ();
// iterate over tree
OctreePointCloudPointVector<PointXYZ>::LeafNodeDepthFirstIterator it2;
const OctreePointCloudPointVector<PointXYZ>::LeafNodeDepthFirstIterator it2_end = octree.leaf_depth_end();
for (it2 = octree.leaf_depth_begin(); it2 != it2_end; ++it2)
{
unsigned int depth = it2.getCurrentOctreeDepth ();
ASSERT_TRUE ((depth == 1) || (depth == 6));
}
// clean up
cloud->points.clear ();
octree.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));
// OctreePointCloudPointVector can store all points..
cloud->push_back (newPoint);
}
// check if all points from leaf data can be found in input pointcloud data sets
octree.defineBoundingBox ();
octree.enableDynamicDepth (leafAggSize);
octree.addPointsFromInputCloud ();
// test iterator
OctreePointCloudPointVector<PointXYZ>::LeafNodeDepthFirstIterator it;
const OctreePointCloudPointVector<PointXYZ>::LeafNodeDepthFirstIterator it_end = octree.leaf_depth_end();
unsigned int leaf_count = 0;
std::vector<int> indexVector;
// iterate over tree
for (it = octree.leaf_depth_begin(); it != it_end; ++it)
{
OctreeNode* node = it.getCurrentOctreeNode ();
ASSERT_EQ (LEAF_NODE, node->getNodeType());
OctreeContainerPointIndices& container = it.getLeafContainer();
if (it.getCurrentOctreeDepth () < octree.getTreeDepth ())
{
ASSERT_LE (container.getSize (), leafAggSize);
}
// add points from leaf node to indexVector
container.getPointIndices (indexVector);
// test points against bounding box of leaf node
std::vector<int> tmpVector;
container.getPointIndices (tmpVector);
Eigen::Vector3f min_pt, max_pt;
octree.getVoxelBounds (it, min_pt, max_pt);
for (i=0; i<tmpVector.size(); ++i)
{
ASSERT_GE (cloud->points[tmpVector[i]].x, min_pt(0));
ASSERT_GE (cloud->points[tmpVector[i]].y, min_pt(1));
ASSERT_GE (cloud->points[tmpVector[i]].z, min_pt(2));
ASSERT_LE (cloud->points[tmpVector[i]].x, max_pt(0));
ASSERT_LE (cloud->points[tmpVector[i]].y, max_pt(1));
ASSERT_LE (cloud->points[tmpVector[i]].z, max_pt(2));
}
leaf_count++;
}
ASSERT_EQ (indexVector.size(), pointcount);
// make sure all indices are within indexVector
for (i = 0; i < indexVector.size (); ++i)
{
#if !defined(__APPLE__)
bool indexFound = (std::find(indexVector.begin(), indexVector.end(), i) != indexVector.end());
ASSERT_TRUE (indexFound);
#endif
}
// compare node, branch and leaf count against actual tree values
ASSERT_EQ (octree.getLeafCount (), leaf_count);
}
}
