Iterating through leaves in an octomap

asked 2014-04-08 18:13:02 -0500

metal gravatar image

Hello Everyone, I am trying to iterate through leaves in an octomap. Here is my code:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h> 
#include<stdio.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include<octomap/OcTreeBase.h>
using namespace std;
using namespace octomap;
double v=0;

int main( int argc, char** argv )

{
  //initialize the node

ros::init(argc, argv, "volume_calc");
ros::NodeHandle node;

AbstractOcTree* tree = AbstractOcTree::read("/home/metal/fuerte_workspace/sandbox/volume_eval/sample.ot");
OcTree* octree = dynamic_cast<OcTree*>(tree);

     for(OcTree::leaf_iterator it = octree->begin_leafs(),
       end=octree->end_leafs(); it!= end; ++it)
{
  //manipulate node, e.g.:
  std::cout << "Node center: " << it.getCoordinate() << std::endl;
  std::cout << "Node size: " << it.getSize() << std::endl;
  std::cout << "Node value: " << it->getValue() << std::endl;
  v=v+(pow(it.getSize(),3));
}

std::cout<<"VOLUME::::"<<v<<endl;
exit(0);


  return 0;
}//main

The octomap was created from point-cloud through octomap_server package. This code calculates the volume of the occupied voxels.

My question is when iterating through leaves, should I be worried about the occupancy of the nodes?. In that case, should I have a nested loop to go through the nodes as well?. An example would be great. Please let me know your thoughts thank you.

edit retag flag offensive close merge delete