ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Iterating through leaves in an octomap

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

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-12-10 16:59:13 -0600

kivrakh gravatar image

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);
      } 
 }
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2014-04-08 18:13:02 -0600

Seen: 2,376 times

Last updated: Dec 10 '22