Iterating through leaves in an octomap
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.