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