use getmetricMin and getMetricMax to get the AABB of the known OcTree…

[simonschmeisser]

… instead of its max dimension

if only a small part of the Octree is actually used and occupied, this will save lots of NarrowPhase collision checks

Note 1: This has basically been rewritten, will squash or please squash on merge
Note 2: getMetricMin and getMetricMax use lazy evaluation/caching and thus need to be called at least once on a non-const version of the octomap::OcTree

---

This change is 

[simonschmeisser]

@jslee02 @j-rivero any comments? do you have an idea why the unit tests would fail?

[simonschmeisser]

ping?

[simonschmeisser]

@Levi-Armstrong you said at ROScon you might know somebody we could ping for this?

[Levi-Armstrong]

@SeanCurtis-TRI Do you have insight into the tests failing? I assume they just need to be updated due to the bounding box changing but have not looked into it.

[simonschmeisser]

the previous version (first commit) was broken as fcl::OcTree::getRootBV is used not only for broad phase but also to calculate the extend of the child nodes. The entire OcTree was therefore scaled down to the extend of its occupied subspace.

The new approach explicitly adds a new broad phase step that checks for overlap with the occupied part of the octomap. Note that the getMetricMin and getMetricMax use lazy evaluation/caching and thus need to be called at least once on a non-const version of the octomap::OcTree