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.
Asked by metal on 2014-04-08 18:13:02 UTC
Answers
This will give the sum of the volumes of free and occupied spaces. If you want to separate the occupied and free space volume, the following code will help you.
for(OcTree::leaf_iterator it = octree->begin_leafs(), end=octree->end_leafs(); it!= end; ++it){
double side_length = it.getSize();
if (octree->isNodeOccupied(*it)){ // occupied leaf node
vol_occ += pow(side_length,3);
}
else { // free leaf node
vol_free += pow(side_length,3);
}
}
Asked by kivrakh on 2022-12-10 17:59:13 UTC
Comments