Setting a bounding box around the robot in octomap

[Anna Mannucci]

Hi to all. I've another question:

has you ever tried to get all the keys of voxel which lies in a box?

I'm trying to get a collision checking around a robot,

so would like to define the size of the box with a point3d and the origin of the box.

Is there something for this purpose or the function needs to be implemented by the user?

Thanks,

Anna

[Ryan Villamil]

This seems what leaf_bbx_iterator is for, lets you iterate through leaf nodes that fall with a given bounding box.  As you iterate you can ask for the keys from the iterator.

[Anna Mannucci]

Thanks for the answer Ryan :) 

I tried but I get segmentation fault. My function is this:

      bool NAVServer::computeBBXKeys(const octomap::OcTree* octree, const point3d& origin, point3d dist, KeySet& ray) 

      {

ray.clear();

      

// Initialization phase -------------------------------------------------------

//controlla che il punto selezionato appartenga all'octree

OcTreeKey key_origin;

if (!octree->coordToKeyChecked(origin, key_origin)) 

{

 ROS_WARN_STREAM("ComputeBBXKeys: tested point is unknown");

 return false;

}

ray.insert(key_origin);

point3d minim = origin - dist;

point3d maxim = origin + dist;

for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(minim,maxim), end = octree->end_leafs_bbx(); it != end; ++it)

 ray.insert(it.getKey());

return true;

      }

Debugging with kdevelop, I've seen that error is at line with bbx_leaf_iterator. I'm using octomap_server. Error is getenv

Why? Some ideas?

Anna

--
You received this message because you are subscribed to the Google Groups "OctoMap developers and users discussion" group.
To unsubscribe from this group and stop receiving emails from it, send an email to octomap+u...@googlegroups.com.
To post to this group, send email to oct...@googlegroups.com.
Visit this group at https://groups.google.com/group/octomap.
For more options, visit https://groups.google.com/d/optout.

[Anna Mannucci]

I found debugging that the problem I get at

for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(minim,maxim), end = octree->end_leafs_bbx(); it != end; ++it)

is when it's called

  template <class NODE,class I>
  bool OcTreeBaseImpl<NODE,I>::coordToKeyChecked(double coordinate, unsigned short int& keyval) const {

    // scale to resolution and shift center for tree_max_val
    int scaled_coord =  ((int) floor(resolution_factor * coordinate)) + tree_max_val; HERE I HAVE SEGMENTATION FAULT

    // keyval within range of tree?
    if (( scaled_coord >= 0) && (((unsigned int) scaled_coord) < (2*tree_max_val))) {
      keyval = scaled_coord;
      return true;
    }
    return false;
  }

I inizialise the octree in this way before I call leaf_bbx_iterator:
          octree = new OcTree(res);
          octree->setProbHit(probHit);
          octree->setProbMiss(probMiss);
          octree->setClampingThresMin(thresMin);
          octree->setClampingThresMax(thresMax);

what is wrong?

[Armin Hornung]

Hello Anna,

A leaf iterator (and similar the leaf_bbx_iterator) can point to a
pruned inner node. These nodes are not at the lowest level and thus have
a larger size than the tree resolution. Accordingly, the key will not
correctly refer to the complete volume of the node. KeyRays,
particularly, only work with leaf node keys.

As a solution, you could expand the complete tree (not preferable, will
consume much more memory), selectively expand relevant nodes, or
determine the size of the iterator's node and insert the correct number
of (virtual) child nodes, each with their own key.

Best regards,
Armin

[Anna Mannucci]

Thanks Armin for the answer. I have solved.

Best,

Anna