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