for(octomap::OcTree::leaf_iterator it = tree.begin_leafs(),end=tree.end_leafs(); it!= end; ++it)
{
// check whether the node is occupied
if (tree.isNodeOccupied(*it)){
//get the side length of the current voxel
double size = it.getSize();
//Compute volume voxel using formula of volume cube
double volumeCurrentVoxel = size * size * size;
volumeTotal = volumeTotal + volumeCurrentVoxel;
}
As my resolution is low in order to have an accurate representation of the object, it's normal to have a little value when I do volumecurrentVoxel = 0.1*0.1*0.1.
My results is under 1. For example: 0.007578 m^3
when I visualize my tree with octoviz I see that under the screen there is the computed volume which is for example 0.55 x 0.8 x 0.58 m^3 which is very different from my result above.
Coud you please help me ?
Thank you
tree.getMetricSize(x1, y1, z1);
So if I got it well (correct me if I'm mistaken), by using getMetricSize, we are getting the size of the entire box which include the object cloud, and by doing as above (compute for every node the volume), we are getting the proper volume of the object cloud ?
Thank you.
float vol_occ = 0; float vol_free = 0; float vol_tot = (bbxMaxX-bbxMinX)*(bbxMaxY-bbxMinY)*(bbxMaxZ-bbxMinZ); point3d min(bbxMinX, bbxMinY, bbxMinZ); point3d max(bbxMaxX, bbxMaxY, bbxMaxZ); OcTree* octree = (OcTree*)octomap_msgs::msgToMap(msg); if (octree){ // can be NULL for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), end=octree->end_leafs_bbx(); 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); } } } float vol_unkn = vol_tot - vol_occ - vol_free;