Coordinates of a Node

[baco...@gmail.com]

Hi there,
Is it possible to get the coordinates of a Node if I only have a Pointer to it (and the Tree)?

I'm trying to do some path planning for a robot with ROS and moveit, and I need to know the distance and direction to the closest obstacle. The distance calculation works fine (with the original code provided by moveit). I modified the code in several moveit_core classes, and so far I also got it to compute the direction - as long as the obstacle is represented as, well, anything but an octree.
However, as soon as the obstacles are represented as octree cells, I'm stuck. I managed to get the octree (octomap::OcTree) and the cell ID (from the distance result of the distance computation, see http://gamma.cs.unc.edu/FCL/fcl_docs/webpage/generated/structfcl_1_1DistanceResult.html) and a Pointer to the Node (octomap::OcTreeNode) that should be closest to the robot. Since the occupied cells cover an area of several square meters, the octree itself is quite too big to be used for any direction calculation.
 The only thing I still need are the coordinates of that node. It would be great if anyone here had any idea how to solve this (or if you could tell me that this is impossible so I can stop trying ;) )

Best Regards,
Alisa

[Felix Endres]

Hi,
Armin is currently unavailable, so I'll try to answer this one.
I do think this is not possible without exhaustive search of the
tree for that node or writing subclasses of tree and node that
store a pointer to the parent (or the coordinates).

Best,
Felix

> --
> 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
> <mailto:octomap+u...@googlegroups.com>.
> To post to this group, send email to oct...@googlegroups.com
> <mailto:oct...@googlegroups.com>.
> Visit this group at http://groups.google.com/group/octomap.
> For more options, visit https://groups.google.com/d/optout.

[Christian Dornhege]

Hi,

As far as I know, yes, this is not possible.
I'm not familiar with FCL, but, what is this cell ID?
If you can get the OcTreeKey for a node that already encodes the
coordinates.

Best,
Christian

[baco...@gmail.com]

Hi,

First of all, thank you for your answers.
I didn't find any octomap documentation on "cell id", so I guess it is just a word the moveit guys came up with. I got it from the DistanceResult object that was filled somewhere in a distance-calculation-function. According to the documentation for DistanceResult (see link in my first post), DistanceResult has a public int mamber named "b1"  with description

 "information about the nearest point in object 1
if object 1 is mesh or point cloud, it is the triangle or point id
if object 1 is geometry shape, it is NONE (-1),
if object 1 is octree, it is the id of the cell "

It took me a while to figure out what this actually means, since I couldn't find anything by googling "cell id" and "octomap" together. Then I stumbled across this piece of code, very deep within the distance function I was calling to fill the DistanceResult object:

dresult->update(...,root1 - tree1->getRoot() , ...)

(dresult is the DistanceResult). This will set dresult.b1 to root1 - tree1->getRoot(). root1 and tree1->getRoot() are pointers, root1 points to the current node, tree1 is the whole tree. So I figured if I have the octree, take the pointer to the root (with getRoot()) and add b1 to that pointer, I might get the address of the Node that caused the previous dresult->update call.
I already tried calling hasChildren and getOccupancy on that adress, and that worked, so at least the pointer points to a valid node object.

I don't think exhaustive search of the tree is an option, since the application has to run in real-time and I have to do several hundred distance-and-direction-calculations per second (among other stuff). I'll try to find out where the octree is created and sneak in some inheritance.

Best Regards,
Alisa

[baco...@gmail.com]

Hi again,
For everyone wondering in the future: I solved it by using a leaf_iterator to iterate over the leaves. Still takes more time than I wanted, though. I still have to check if this is good(=fast) enough for what I'm trying to do, but I'm happy to have found a way to get the coordinates of the node at last, even if it's probably to slow.
The whole thing looks like this:

double d = fcl::distance(o1,o2,dist_req,dist_result); //o1 and o2 are fcl::CollisionObjects, o2 belongs to the robot. See https://github.com/ros-planning/moveit_core/blob/hydro-devel/collision_detection_fcl/src/collision_common.cpp#L440

//The following goes right under line 449, in the if(cdata->res_->distance > d) block. cd1 is already computed above in line 364.
if (cd1->ptr.obj->shapes_[0]->type == shapes::OCTREE){
  shapes::ShapeConstPtr shapeCP = cd1->ptr.obj->shapes_[0];
  const shapes::OcTree* ocShape = dynamic_cast<const shapes::OcTree*> (shapeCP.get());
  const octomap::OcTree* oct = ocShape->octree.get();
  const octomap::OcTreeNode* rootNode = oct->getRoot();
  const octomap::OcTreeNode* closestNode = treeRott + dist_result.b1;

  octomap::OcTreeBaseImp<octomap::OcTreeNode, octomap::AbstracOccupancyOcTree>::iterator ocTreeIt, end;
  ocTreeIt = oct->begin(0);
  bool found = false;
  for (end = oct->end(); !found && ocTreeIt != end; ++ocTreeIt){
    if (closestNode == &(*ocTreeIt)){
      found = true;
      //get position of node with ocTreeIt.getX() / getY() / getZ()
    }
  }
}


Best Regards,
Alisa

[Armin Hornung]

Am 25.08.2014 09:06, schrieb baco...@gmail.com:
> Hi again,
> For everyone wondering in the future: I solved it by using a
> leaf_iterator to iterate over the leaves. Still takes more time than I
> wanted, though. I still have to check if this is good(=fast) enough
> for what I'm trying to do, but I'm happy to have found a way to get
> the coordinates of the node at last, even if it's probably to slow.

Sure, traversing the complete octree and checking every node if it's the
right one won't be efficient. You will probably be better off exposing
the right properties of the node (size, position) directly from the FCL
collision check. The data must be there and already used for the
collision check / distance computation.

Cheers,
Armin

--
Dr. Armin Hornung
Humanoid Robots Lab, Albert-Ludwigs-Universität Freiburg
Contact: http://www.informatik.uni-freiburg.de/~hornunga