Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109

peci1 · 2019-05-28T16:18:34Z

This PR fixes a multitude of problems the current implementations of intersectsRay() had.

In Box, even according to the link to the used method, the approach should only work for axis-aligned boxes. But bodies::Box can be arbitrarily transformed. So I instead strip rotation from both the box and the ray, which gives the same results. The old implementation would fail every time the untransformed min corner ceased to be the min corner of the transformed box.

For Cylinder, there were a few cornercases when incorrect number of intersections were returned (e.g. when the ray is tangential to the cylinder or when it goes through the edge between base and sides).

For ConvexMesh, there were several problems. Many of them arose from a wrong approach to incorporate padding. Also, if a ray hit the edge of two triangles, it was reported twice.

I also restructured the tests for both containsPoint() and intersectsRay() and added a lot of new tests - both randomized and testing corner cases. There is also a regression test BoxRayIntersection::Regression109 which fails on current melodic-devel and succeeds with this test.

Aside from the fixes in intersectsRay(), I also fixed Cylinder::samplePointInside() and ConvexMesh::containsPoint() whose correct working is needed by the unit tests.

peci1 · 2019-05-28T16:19:45Z

This would deserve a few tests, they're coming tomorrow. For the time being I verified the algorithm in RViz.

rhaschke

Generally, this looks good to me.
Please provide a description for this PR, mentioning that this depends on other PRs.
Please also provide the promised unit tests (one of them should be failing before this fix).

src/bodies.cpp

peci1 · 2019-08-09T14:22:51Z

I added PR description and separated this PR from the others as it doesn't depend on them semantically. I'll have a look at the tests.

peci1 · 2020-03-25T01:51:01Z

I had a look at this again, rebased on current melodic-devel, and added a lot of tests. First, I only added box-ray tests, but then I decided to add tests for all bodies. And along the way, I've discovered and fixed a lot of other bugs in various more or less related functions, like containsPoint() and samplePoint(). Check out the updated title and description of this PR.

I would've sent these other fixes as another PRs, but the tests in this one would fail without them. But if it's desired, I can take the effort and split this into more PRs - I just do not want to take this effort with the tests, so I'd separate them as the "final" PR, and the prior ones would come without tests.

I did some API/ABI breaks here.

First, as requested by @rhaschke , I removed the unnecessary internal fields normalL_, normalW_ and normalH_ of bodies::Box. I instead added invPose_.
When I was already breaking API/ABI, I also changed MeshData by adding one more field.
Not really API/ABI break, but I changed the meaning of corner1_ and corner2_ protected members of bodies::Box. It should not affect anything but potential user code that would access these fields. But as they had no docs before, I think the chance there is some other code using them is pretty minimal.

If API/ABI breaks are not desired for melodic-devel, 1) can be avoided completely (and the API break be left to noetic), and 2) can also be worked around somehow (last resort would be the translation-unit-private global variable ugly trick).

For some reason, CI did not trigger here (maybe because of the pending requested changes?), but here's mine which shows okay: https://travis-ci.com/github/peci1/geometric_shapes . I've also run a local test with this PR and the whole Moveit repo. It ran fine, except for a few ushort_test/MeshFilterTestUnsignedShort.unsigned_short tests, which however fail for me even on melodic-devel branch of geometric_shapes.

As with my other PRs, this may of course break user code that counted with the buggy behavior. But (thanks to the tests), I'm pretty sure that if the library is used correctly, it should not bring any new problems.

v4hn

Please address the inline comments. Aside from them, I approve.

src/bodies.cpp

v4hn · 2020-04-24T12:32:33Z

Lastly, this patch conflicted with one of the others I merged. Could you please rebase?

addressed

peci1 · 2020-04-24T15:44:22Z

I've made the requested changes.

peci1 · 2020-04-24T17:17:07Z

Thanks for the reviews, @v4hn ;)

v4hn · 2020-04-24T17:29:29Z

On Fri, Apr 24, 2020 at 10:17:21AM -0700, Martin Pecka wrote: Thanks for the reviews, @v4hn ;)

Thanks for your continuous effort with these patches! I still fail to understand how all these issues could ever have made it through the years...

peci1 · 2020-04-24T17:31:49Z

I think insisting on good tests for PRs is the way to prevent most of such errors for the future. I've seen moveit repos already report some kind of test coverage... What about this repo? Is it planned?

v4hn · 2020-04-24T21:37:10Z

#120 :-)

rhaschke

Dear @peci1,
please adapt multiple consecutive uses of set*() to set*Dirty() after having merged #126. Otherwise, this looks good. Thanks a lot for all your effort on this!

rhaschke · 2020-04-25T12:49:28Z

include/geometric_shapes/bodies.h


-  Eigen::Vector3d corner1_;
-  Eigen::Vector3d corner2_;
+  Eigen::Vector3d corner1_;  //!< The translated, but not rotated min corner


What about renaming these vectors to min_corner_ and max_corner_?
As you renamed normal[L,W,H] anyway, ABI and API is broken anyway and we need to target this to Noetic only.

Okay, I can rename them for noetic. As I'd still like to see this in melodic, should I split this PR into two and try to make this one compatible with melodic? In my view, this approach would be easier than first merging to noetic and then backporting...

As discussed earlier, the API/ABI breaks can be avoided at the cost of minor uglification of code.

To maintain ABI you would need to keep the axis vectors instead of invRot_. More ugly, but ok.
The other addition I have seen is std::map<unsigned int, unsigned int> triangle_for_plane_.
Didn't look for what it is actually used. Replacing this by a global variable, I would actually not support. Why is it needed now? Just to be faster?

Walking on a thin ice, I guess a Matrix3d could take up the same amount of memory as three Vector3ds. But that could run in many issues once memory alignment comes into play. So I rather give up this idea.

invRot would have to be recomputed every time it is used, but as it is only a transpose, it shouldn't be that bad in melodic. I can also imagine storing the rows/cols of the inverted matrix in the abandoned vectors, but I bet the performance could be even worse than transposing it where needed.

The triangle_for_plane is really required for efficiency - otherwise mesh-ray intersection tests would need to re-enumrate over all triangles to find which plane the current one belongs to. The global variable trick is something Gazebo people are willing to accept if it is just "temporary" to get a feature to a released version; the development version then implements the nicer, but ABI-breaking version. An example is here: gazebosim/gazebo-classic@415be40 . It creates a global map, which stores references to created objects (ConvexMesh in this case) and maps them to the structure that is needed to be added. Ugly, but works.

Actually, I'd consider making the whole MeshData PIMPL for noetic (i.e. moving it to the .cpp file), so that storing additional data will not cause an ABI break any more. The same could be done for the other bodies, too, but there would be the additional cost of dynamic memory allocation. The ConvexMesh allocates memory anyways, so adding one more unique_ptr should not be a problem.

I implemented ABI compatibility using the global variable.

include/geometric_shapes/bodies.h

src/bodies.cpp

Even according to the link to the used method, the approach should only work for axis-aligned boxes. But bodies::Box can be arbitrarily transformed. So I instead strip rotation from both the box and the ray, which gives the same results. The old implementation would fail every time the untransformed min corner ceased to be the min corner of the transformed box. # Conflicts: # src/bodies.cpp

…within the box. A false second intersection was returned in that case, but as the ray starts inside the box, there should only be one intersection.

codecov-io · 2020-04-26T00:08:34Z

Codecov Report

Merging #109 into melodic-devel will increase coverage by 2.98%.
The diff coverage is 97.77%.

@@                Coverage Diff                @@
##           melodic-devel     #109      +/-   ##
=================================================
+ Coverage          46.24%   49.22%   +2.98%     
=================================================
  Files                 11       11              
  Lines               1691     1684       -7     
=================================================
+ Hits                 782      829      +47     
+ Misses               909      855      -54

Impacted Files	Coverage Δ
include/geometric_shapes/bodies.h	`86.44% <ø> (-0.66%)`	⬇️
src/bodies.cpp	`71.13% <97.77%> (+9.01%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update adacb1c...ed4cf13. Read the comment docs.

peci1 · 2020-04-26T00:20:24Z

@rhaschke I did all the changes you requested. ABI compatibility with melodic is achieved by the very last commit.

Thanks for the deep insights. The code looks much better now.

peci1 · 2020-04-26T00:22:28Z

I also restructured the commit history. Now I consider all the commits valuable and further squashing is not needed.

rhaschke

Some minor remarks left.
As I will not cherry-pick to noetic, but revert the last commit, please remove the comment
Do not cherry-pick this commit to noetic. from it.
Thanks a lot for all this work. Hope, we can merge this later today.

src/bodies.cpp

…int by the transform of the body.

…in case the ray is touching the cylinder side. Also fixed the case when the ray goes exactly at the edge between base and sides.

…y().

The reported problems no longer occur.

…d some other bugs.

…h::intersectsRay().

peci1 · 2020-04-26T10:49:13Z

@rhaschke I addressed your comments.

rhaschke · 2020-04-26T14:27:56Z

@peci1, thank you very much. All your PRs are a great improvement of this code base!
If I may ask for even more, here are my suggested next steps:

use of set*Dirty() functions (for Melodic)
API cleanup (for Noetic)
- Consider moving non-trivial functions from .h to .cpp.
  Usually, only one- or two-liners deserve inlining.
- min_corner_ and max_corner_ as suggested in Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109 (comment)
- pimpl pattern for MeshData as suggested in Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109 (comment)
any other improvements you can think of 😉

rhaschke · 2020-04-26T14:42:45Z

I added a noetic-devel branch, just in case you need it ...

peci1 force-pushed the fix-box-ray-intersection branch 2 times, most recently from 82ec8c0 to fcf33ca Compare May 29, 2019 11:03

rhaschke previously requested changes Aug 9, 2019

View reviewed changes

src/bodies.cpp Outdated Show resolved Hide resolved

src/bodies.cpp Show resolved Hide resolved

peci1 force-pushed the fix-box-ray-intersection branch 2 times, most recently from 9f2429b to d0d5b72 Compare August 9, 2019 14:21

peci1 force-pushed the fix-box-ray-intersection branch from d0d5b72 to 60260af Compare August 9, 2019 14:28

peci1 force-pushed the fix-box-ray-intersection branch 3 times, most recently from a1fa4da to 522340c Compare March 24, 2020 23:01

peci1 changed the title ~~Fixed computation of ray-box intersections.~~ Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh Mar 24, 2020

peci1 force-pushed the fix-box-ray-intersection branch 2 times, most recently from 4be04c6 to 828c146 Compare March 24, 2020 23:23

peci1 mentioned this pull request Mar 25, 2020

Wrong assumption about planes of a ConvexMesh in case padding is used #127

Open

v4hn approved these changes Apr 24, 2020

View reviewed changes

src/bodies.cpp Outdated Show resolved Hide resolved

src/bodies.cpp Outdated Show resolved Hide resolved

src/bodies.cpp Outdated Show resolved Hide resolved

peci1 force-pushed the fix-box-ray-intersection branch from a7ffd0c to 43cb6f0 Compare April 24, 2020 15:43

v4hn added the awaits 2nd approval label Apr 24, 2020

rhaschke reviewed Apr 25, 2020

View reviewed changes

rhaschke mentioned this pull request Apr 25, 2020

Added constructors of bodies from the corresponding bounding shapes #128

Merged

peci1 added 2 commits April 25, 2020 23:47

Fixed computation of Box::intersectsRay() in case the ray originates …

6c9b24a

…within the box. A false second intersection was returned in that case, but as the ray starts inside the box, there should only be one intersection.

peci1 force-pushed the fix-box-ray-intersection branch from 43cb6f0 to 2f39342 Compare April 26, 2020 00:02

rhaschke reviewed Apr 26, 2020

View reviewed changes

src/bodies.cpp Outdated Show resolved Hide resolved

src/bodies.cpp Outdated Show resolved Hide resolved

src/bodies.cpp Outdated Show resolved Hide resolved

peci1 added 17 commits April 26, 2020 12:39

Fixed ConvexMesh::containsPoint() to work correctly with padding.

15b31c4

Fixed Cylinder::samplePointInside() not transforming the generated po…

0d68ede

…int by the transform of the body.

Fixed Cylinder::intersectsRay() to correctly return one intersection …

a627e6f

…in case the ray is touching the cylinder side. Also fixed the case when the ray goes exactly at the edge between base and sides.

Fixed a case which could result in unwanted NaNs in Box::intersectsRa…

ab43142

…y().

Stopped preventing GCC optimizations on Box::intersectsRay().

c399036

The reported problems no longer occur.

Fixed computation of ConvexMesh::intersectsRay() for padded meshes an…

c3c4643

…d some other bugs.

Prevent reporting identical intersections multiple times in ConvexMes…

5954985

…h::intersectsRay().

Improved documentation.

2dd06d8

Moved all ray intersection tests to a dedicated test file.

4ff0679

Refactored intersection tests for better readability.

c888ccd

Added more intersectsRay() tests.

a99602f

Added more containsPoint() tests.

ce88eca

Kinetic compatibility.

2ad1bf8

Utilize setDimensionsDirty() to increase performance.

91bb2cd

Vectorized and simplified bodies::Box::intersectsRay().

1018336

Refactored intersection code.

ceb6a42

ABI compatibility for melodic.

ed4cf13

peci1 force-pushed the fix-box-ray-intersection branch from 2f39342 to ed4cf13 Compare April 26, 2020 10:48

rhaschke merged commit 54fbcf1 into moveit:melodic-devel Apr 26, 2020

This was referenced Apr 27, 2020

Utilize setScaleDirty() and setPaddingDirty() to improve performance moveit/moveit#2038

Merged

noetic-abi-breaks #136

Closed

rhaschke mentioned this pull request May 25, 2020

Box/ray intersections are broken in 0.5.4 #86

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109

Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109

peci1 commented May 28, 2019 •

edited

peci1 commented May 28, 2019

rhaschke left a comment

peci1 commented Aug 9, 2019

peci1 commented Mar 25, 2020

v4hn left a comment

v4hn commented Apr 24, 2020

peci1 commented Apr 24, 2020

peci1 commented Apr 24, 2020 •

edited

v4hn commented Apr 24, 2020 via email

peci1 commented Apr 24, 2020

v4hn commented Apr 24, 2020

rhaschke left a comment

rhaschke Apr 25, 2020

peci1 Apr 25, 2020

rhaschke Apr 25, 2020

peci1 Apr 25, 2020

peci1 Apr 26, 2020

codecov-io commented Apr 26, 2020 •

edited

peci1 commented Apr 26, 2020

peci1 commented Apr 26, 2020

rhaschke left a comment

peci1 commented Apr 26, 2020

rhaschke commented Apr 26, 2020

rhaschke commented Apr 26, 2020

Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109

Fixed computation of intersectsRay() for Cylinder, Box and ConvexMesh #109

Conversation

peci1 commented May 28, 2019 • edited

peci1 commented May 28, 2019

rhaschke left a comment

Choose a reason for hiding this comment

peci1 commented Aug 9, 2019

peci1 commented Mar 25, 2020

v4hn left a comment

Choose a reason for hiding this comment

v4hn commented Apr 24, 2020

peci1 commented Apr 24, 2020

peci1 commented Apr 24, 2020 • edited

v4hn commented Apr 24, 2020 via email

peci1 commented Apr 24, 2020

v4hn commented Apr 24, 2020

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Apr 25, 2020

Choose a reason for hiding this comment

peci1 Apr 25, 2020

Choose a reason for hiding this comment

rhaschke Apr 25, 2020

Choose a reason for hiding this comment

peci1 Apr 25, 2020

Choose a reason for hiding this comment

peci1 Apr 26, 2020

Choose a reason for hiding this comment

codecov-io commented Apr 26, 2020 • edited

Codecov Report

peci1 commented Apr 26, 2020

peci1 commented Apr 26, 2020

rhaschke left a comment

Choose a reason for hiding this comment

peci1 commented Apr 26, 2020

rhaschke commented Apr 26, 2020

rhaschke commented Apr 26, 2020

peci1 commented May 28, 2019 •

edited

peci1 commented Apr 24, 2020 •

edited

codecov-io commented Apr 26, 2020 •

edited