Method extractPointsAtLevel

tools/octree_viewer.cpp:364–421

\brief Extracts all the points of depth = level from the octree * */

   *
   */
  void extractPointsAtLevel(int depth)
  {
    displayCloud->points.clear();
    cloudVoxel->points.clear();

    pcl::PointXYZ pt_voxel_center;
    pcl::PointXYZ pt_centroid;
    std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
    double start = pcl::getTime ();

    for (pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::FixedDepthIterator tree_it = octree.fixed_depth_begin (depth);
         tree_it != octree.fixed_depth_end ();
         ++tree_it)
    {
      // Compute the point at the center of the voxel which represents the current OctreeNode
      Eigen::Vector3f voxel_min, voxel_max;
      octree.getVoxelBounds (tree_it, voxel_min, voxel_max);

      pt_voxel_center.x = (voxel_min.x () + voxel_max.x ()) / 2.0f;
      pt_voxel_center.y = (voxel_min.y () + voxel_max.y ()) / 2.0f;
      pt_voxel_center.z = (voxel_min.z () + voxel_max.z ()) / 2.0f;
      cloudVoxel->points.push_back (pt_voxel_center);

      // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode
      if (octree.getTreeDepth () == static_cast<unsigned int>(depth))
      {
        auto* container = dynamic_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode ());

        container->getContainer ().getCentroid (pt_centroid);
      }
      // Else, compute the centroid of the LeafNode under the current BranchNode
      else
      {
        // Retrieve every centroid under the current BranchNode
        pcl::octree::OctreeKey dummy_key;
        pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids;
        octree.getVoxelCentroidsRecursive (dynamic_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids);

        // Iterate over the leafs to compute the centroid of all of them
        pcl::CentroidPoint<pcl::PointXYZ> centroid;
        for (const auto &voxelCentroid : voxelCentroids)
        {
          centroid.add (voxelCentroid);
        }
        centroid.get (pt_centroid);
      }

      displayCloud->points.push_back (pt_centroid);
    }

    double end = pcl::getTime ();
    printf("%zu pts, %.4gs. %.4gs./pt. =====\n",
           static_cast<std::size_t>(displayCloud->size()),
           end - start,
           (end - start) / static_cast<double>(displayCloud->size()));

    update();
  }