search

moveit / geometric_shapes

Representation of geometric shapes
57 stars 92 forks source link

Improve accuracy of computed oriented bounding boxes #232

Closed peci1 closed 1 year ago

peci1 commented 1 year ago

In case one OBB is fully contained inside the other, merging them together will now result in the larger one being returned exactly (normally, FCL yields a larger OBB than the original one).

This change is needed to keep multiple merged OBBs coming from a robot model sane. I don't know exactly why the original PR adding OBBs support did not contain this improvement - my code in robot_body_filter has been doing this for the last 4 years: https://github.com/peci1/robot_body_filter/blob/2c13fc8b0aa5ae7ad71a336a3916f53171c8d9f5/src/utils/obb.cpp#L122-L140 .

With this change, our tracked robot model OBB is kept within +- 2 meters range. Without this change, after being composed by merging 40+ OBBs with various rotations, the overall OBB dimensions are in the range of 40 kilometers! My understanding is that FCL first computes an average orientation of both OBBs (altough a bit weird by averaging coordinates of the quaternions) and then finds the smallest box with this orientation. This way, after merging, the resulting box may be up to sqrt(3)-times larger. Thus a model composed of 40 OBBs can have the resulting OBB up to sqrt(3)^40 larger than necessary. This PR stops this infinite growth quite early once an OBB large enough to contain the whole model is created.

I added a few tests that fail when fully relying on FCL to merge the two OBBs.

peci1 commented 1 year ago

Dear maintainers, merging this PR would help resolving build regression in https://github.com/peci1/robot_body_filter/issues/23 which currently blocks Noetic sync: https://discourse.ros.org/t/preparing-for-noetic-sync-2023-04-13/30848 . Would you please have a look at it shortly so that we can move on? Thanks! @AndyZe you helped reviewing the original PR adding OBB support

peci1 commented 1 year ago

Thanks for merging, @rhaschke . Could you please do another noetic release with this fix, so that we can use it in our package?

And also please consider a melodic backport and release (a simple cherry pick should do). Thanks.

rhaschke commented 1 year ago

Can you please prepare the backport? Thanks.

peci1 commented 1 year ago

Sure! #233.

rhaschke commented 1 year ago

https://github.com/ros/rosdistro/pull/36804