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