Fix rss and improve octree tests

[c-andy-martin]

There were several bugs in RSS handling that impacted octree distance checks.
There was also a bug in fcl::octree impacting when nodes were considered free/occupied.
Fix the bugs, and improve both RSS and octree distance check testing.

---

This change is 

[SeanCurtis-TRI]

Sorry about leaving this PR in limbo for a month. I'ts on my plate now.

Reviewable status: 0 of 12 files reviewed, all discussions resolved

[SeanCurtis-TRI]

+@SeanCurtis-TRI

Reviewable status: 0 of 12 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)

[SeanCurtis-TRI]

This was an incredibly hard PR to review. It contains so many independent changes. In the future I would definitely advocate keeping them separate (even though it does increase the PR overhead). It means that an issue on one aspect doesn't hold up the other aspects.

One meta-note: I need you to curate the commits to merge the RSS fixes and unit tests into a single commit. Or, at the very least, the unit test that tests your fixes should be in the same commit as the test. :) Simpler just to put it all together.

Reviewed 12 of 12 files at r1.
Reviewable status: all files reviewed, 24 unresolved discussions (waiting on @c-andy-martin)

---

include/fcl/geometry/octree/octree.h, line 71 at r1 (raw file):
I'm not persuaded as to this change. From your commit message:

However, the default was a probability of 0.0, which is impossible to represent in an octree (since it stores the logodds).

1. The logodds is define (in the code) as log(p / (1-p)). The image of that function definitely includes zero. logodds(0) --> 0.
2. You haven't changed the default value for either of these thresholds. It's still zero. And given point 1, this doesn't seem to actually make anything better.
3. I'm uncomfortable bleeding the logodds implementation into FCL when octomap has gone out of its way to provide a probability API. The only true justification for changing the implementation would be for demonstrable performance reasons.
4. I'm not seeing any new unit tests to support this change -- presumably there's a scenario that would fail without this change. That would go a long way to justify this change. Right now, I can't imagine it.

---

include/fcl/math/geometry.h, line 96 at r1 (raw file):

template <typename S>
FCL_EXPORT
void generateCoordinateSystem(Matrix3<S>& axis);

nit: Given the work you did, it would be great to document the semantics of this function here. The things that jump out to me is:

1. This creates an orthonormal basis.
2. The first column of the input matrix is taken as the x-axis of the new basis.
3. While the x-axis doesn't have to be strictly unit length, it has to be "sufficiently" far from zero length to inspire confidence that it's a meaningful value. (In this case, greater than 1e-3).
4. The y- and z- axes will be arbitrarily oriented around the given x-axis.

---

include/fcl/math/geometry-inl.h, line 649 at r1 (raw file):

{
  // Axis column 0 should be passed already normalized.
  assert(std::abs(1.0 - axis.col(0).squaredNorm()) < 1e-3);

nit: This is a very loose tolerance for the x-axis. Using this basis, you'll be introducing skew into transformed vertices relatively quickly. You still need to normalize this both for the output and prior to using it in the cross product.

---

include/fcl/math/geometry-inl.h, line 669 at r1 (raw file):

  }
  axis.col(1).normalize();
  axis.col(2).noalias() = axis.col(0).cross(axis.col(1));

nit: This should be normalized just in case. Even with the correction of the x-axis (noted above), we can still introduce rounding error via a cross product that can be corrected by a re-normalization. Let's make the best quality basis possible.

---

include/fcl/math/bv/RSS.h, line 47 at r1 (raw file):

{

/// @brief A class for rectangle sphere-swept bounding volume

BTW According to the original paper (http://gamma.cs.unc.edu/SSV/ssv.pdf) RSS wouldbe "rectangle swept sphere" and not "rectangle sphere-swept". Again, as long as we're here, could we hit that as well?

---

include/fcl/math/bv/RSS.h, line 58 at r1 (raw file):

  /// principle directions of the box. We assume that the first column
  /// corresponds to the axis with the longest box edge, second column
  /// corresponds to the shorter one and the third coulumn corresponds to the

btw I could I persuade you to fix this typo while you're here? "coulumn" vs "column".

---

include/fcl/math/bv/RSS.h, line 59 at r1 (raw file):

  /// corresponds to the axis with the longest box edge, second column
  /// corresponds to the shorter one and the third coulumn corresponds to the
  /// shortest one.

BTW Also, this language that suggests there's the third column corresponds to the "shortest" axis is absurd because the RSS is a 2D rectangle embedded in 3D space and not a 3D rectangular solid like this language suggests. In fact, we expect the third column to be perpendicular to the rectangle. That's a much more interesting characterization.

---

include/fcl/math/bv/RSS.h, line 63 at r1 (raw file):
nit: You have to be careful with the definition of "minimum". Because the rectangle can be arbitrarily oriented in another frame, it may not be the minimum in that frame.

So, perhaps we need to elaborate further on the class:

RSS defines a rectangle swept sphere. The rectangle is defined in the frame T. It has one corner positioned at the origin of frame T. It lies on T's z = 0 plane. Its longest dimension extends in the +x direction, and its shorter dimension in the +y direction.

The RSS is then posed in another frame F. axis is the relative orientation between frame T and F and To is the position of T's origin in Frame F.

In the notation we've been stealing from Drake:

• To would be p_FoTo_F (the displacement vector from F's origin to T's origin, measured and expressed in frame F).
• axis would be R_FT (the relative orientation between frames F and T such that, e.g., p_FoQ_F = R_FT * p_FoQ_T).

Typically, FCL is lacking in rigor in talking about what the quantities are and how are they measured and expressed. I wouldn't remotely say we should change the names of axis or To (as they are part of public APIs that would cause all sorts of breakages). But we can certainly be more precise in the documentation.

Note: while this class can't know anything about the semantics of Frame F, it's probably still worth putting a note that frame F is, e.g., the frame of the geometry being bound.

---

include/fcl/math/bv/RSS.h, line 113 at r1 (raw file):

  const Vector3<S> center() const;

  /// @brief Set the To of the RSS from the center coordinates

nit missing period.

---

include/fcl/math/bv/RSS.h, line 115 at r1 (raw file):

  /// @brief Set the To of the RSS from the center coordinates
  ///
  /// Useful because To of an RSS is the minimum corner of the rectangle, not the center.

nit: Let's limit all of the new lines to 80-column width.

---

include/fcl/math/bv/RSS.h, line 115 at r1 (raw file):

  /// @brief Set the To of the RSS from the center coordinates
  ///
  /// Useful because To of an RSS is the minimum corner of the rectangle, not the center.

nit: Based on previous comment, we can keep the word "minimum", but I'd suggest putting it in quotes here (to indicate that its meaning depends on context.)

---

include/fcl/math/bv/utility-inl.h, line 843 at r1 (raw file):

    bv2.axis.noalias() = tf1.linear() * bv1.axis;

    bv2.r = bv1.extent[2];

BTW: The RSS documentation declares correlation with rectangle measures and local axes. Looking just at this code, it isn't clear that this contract is maintained. However, if one reads the OBB documentation, one can see that OBB makes the same promise.

How do you feel about putting a note here indicating that RSS's documented requirement relies on OBB's documented requirement (vis a vis axes and extents)?

---

include/fcl/math/bv/utility-inl.h, line 847 at r1 (raw file):

    bv2.l[1] = 2 * (bv1.extent[1]);

    bv2.setToFromCenter(tf1 * bv1.To);

nit: as long as you're changing this, let's swap bv1.To with bv1.center().

---

test/test_fcl_math.cpp, line 141 at r1 (raw file):

  pn[0] << 0, 0, 0;
  pn[1] << 1, 0, 0;
  fit(pn, 1, rss);

nit: This function seems to be testing too much.

1. First we have a bunch of tests exercising the fit function.
2. Then we're testing the conversion.
3. Then distance
4. The setting the position of the RSS's rectangle.

This single test would benefit from being broken up into tests that more individually test those very different modes.

(Perhaps with a TODO listing the things that haven't been tested on RSS (e.g., overlap, contain, operator+, size, etc.)

---

test/test_fcl_math.cpp, line 142 at r1 (raw file):

  pn[1] << 1, 0, 0;
  fit(pn, 1, rss);
  EXPECT_TRUE(rss.center() == pn[0]);

nit: All of these would be better as EXPECT_EQ.

---

test/test_fcl_math.cpp, line 147 at r1 (raw file):

  EXPECT_TRUE(rss.depth() == 0.0);
  EXPECT_TRUE(rss.axis.isApprox(Matrix3<S>::Identity()));
  fit(pn, 2, rss);

All of these specific cases seem to be quite redundant. This doesn't look like a strategic test of the fit function. If I'm mistaken, some notes in the code indicating the purpose of this strategy will help educate future readers.

---

test/test_fcl_math.cpp, line 259 at r1 (raw file):

GTEST_TEST(FCL_MATH, rss)
{
//  test_rss<float>();

nit Any reason you're including brand new, commented-out code?

---

test/test_fcl_octomap_distance.cpp, line 124 at r1 (raw file):

{
#ifdef NDEBUG
  octomap_distance_test_BVH<RSS<S>>(15);

nit: This represents a huge change in tests. The CI compute budget given as an open source project is quite limited. And these tests aren't cheap. Given that we're simply generating a bunch of random transforms, it's not clear to me that doing more transforms per test execution is inherently better than just allowing the aggregate effect of many, many CI runs.

---

test/test_fcl_octomap_distance.cpp, line 195 at r1 (raw file):

  {
    // Be sure to test identity
    transforms[n - 1] = Transform3<S>::Identity();

Why are we testing transforms three times? I don't buy this. And if it's simply because I'm not seeing it, a quick doc explaining why it makes sense will alleviate future confusion.

My best guess is that you want to make sure you have at least one case where tf1 = tf2 = Identity but aren't confident that a single value in the center would align. Is that it? To be perfectly honest, I'm hard-pressed to believe that it has particular value. Both geometries being the identity will tell us the least about the code being exercised. If it works across random pairs of transforms, failure when they are both identities would require some kind of adversarial bug.

---

test/test_fcl_octomap_distance.cpp, line 215 at r1 (raw file):

    DefaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
    DefaultDistanceFunction(&obj2, &obj1, &cdata1b, dist1b);
    EXPECT_NEAR(dist1, dist1b, 1e-6);

nit: This tolerance should be expressed in terms of the scalar type. See constants<S>::eps() or constants<S>::eps78() or constants<S>::eps12() (for a very loose tolerance).

---

test/test_fcl_octomap_distance.cpp, line 235 at r1 (raw file):

      DefaultDistanceData<S> cdatab;
      S dist = std::numeric_limits<S>::max();
      DefaultDistanceFunction(&obj1, boxes[j], &cdatab, dist);

What does this code do? It runs, but nothing is asserted.

---

test/test_fcl_octomap_distance.cpp, line 240 at r1 (raw file):

    delete manager;

    std::cout << dist1 << " " << dist2 << std::endl;

BTW Could I talk you into deleting this console dump?

---

test/test_fcl_octomap_distance.cpp, line 251 at r1 (raw file):

    // the nearest points may be tangential and the equal to the (potentially
    // fake) signed distance returned by the distance check.
    if (dist1 >= 0.0)

Do you mean for this to include "touching"? If not dist1 > 0.0.

---

test/test_fcl_utility.h, line 605 at r1 (raw file):

      for(int z = -20; z < 20; z++)
      {
        tree->setNodeValue(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), tree->getClampingThresMaxLog());

I have one major concern about this change. The commit message suggests that this change is merely about pruning. I strongly suspect the resultant tree is actually different.

As I see it, these two for loops traverse overlapping spaces (the second for loop is a smaller, inset region). The outer loop "adds" occupancy to the whole map and the inner removes it from the inset region.

Using updateNode this is an "additive","subtractive" approach. Using setNodeValue() uses an overwrite approach. Furthermore, the mapping of the former bool values to probabilities creates different values in the cells than would be defined by the threshold values. It's impossible for me to believe that the variance in operators and operands produce an equivalent result.

Therefore, I believe this change needs to be well argued that it's a good change and it can't just be about pruning.

[c-andy-martin]

Apologies. Feel free next time to simply refuse me and I will break it up prior to your review to avoid this happening again. As it is, I hate to do it now that you've spent the time leaving all the feedback. I have curated the commits as you indciate. I have only responded to one feedback, and will get to the rest tomorrow.

Reviewable status: 12 of 13 files reviewed, 24 unresolved discussions (waiting on @c-andy-martin and @SeanCurtis-TRI)

---

include/fcl/geometry/octree/octree.h, line 71 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm not persuaded as to this change. From your commit message:

However, the default was a probability of 0.0, which is impossible to represent in an octree (since it stores the logodds).

1. The logodds is define (in the code) as log(p / (1-p)). The image of that function definitely includes zero. logodds(0) --> 0.
2. You haven't changed the default value for either of these thresholds. It's still zero. And given point 1, this doesn't seem to actually make anything better.
3. I'm uncomfortable bleeding the logodds implementation into FCL when octomap has gone out of its way to provide a probability API. The only true justification for changing the implementation would be for demonstrable performance reasons.
4. I'm not seeing any new unit tests to support this change -- presumably there's a scenario that would fail without this change. That would go a long way to justify this change. Right now, I can't imagine it.

1. Zero is not in the domain of logarithm, so passing probability of 0 to logodds will result in log(0/1) which will result in -inf, nan or even an exception depending on the hardware one is running on. Therefore one must not use a probabilityof 0 with octomap, as it doesn't map onto logodds.

2. I have changed the meaning of the thresholds to no longer be probability, but log_odds. All occupancy octrees in octomap store the actual cell probability in logodds format. So you will see in the implementation where I now compare with getLogOdds() (which does no computation) to the log odds threshold. Its OK to bleed the concept of log odds into FCL octree here as it is part of the private implementation. Also log odds is an important part of how occupancy octree's work in octomap, as that is how the occupancy threshold in octomap already works. (Note that there is no corresponding free threshold in octomap, which is presumably the reason it was added to FCL at some point). Note that all the public APIs in FCL::octree continue to take probability, and those are correctly converted to logodds using octomap::logodds

3. Turning a probability into a logodds involves calling std::log, which isn't very efficient when doing many comparisons, as would be done in a distance query.

4. I will provide a test that would fail prior to this change.

[SeanCurtis-TRI]

Reviewable status: 12 of 13 files reviewed, 24 unresolved discussions (waiting on @c-andy-martin and @SeanCurtis-TRI)

---

include/fcl/geometry/octree/octree.h, line 71 at r1 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

1. Zero is not in the domain of logarithm, so passing probability of 0 to logodds will result in log(0/1) which will result in -inf, nan or even an exception depending on the hardware one is running on. Therefore one must not use a probabilityof 0 with octomap, as it doesn't map onto logodds.

2. I have changed the meaning of the thresholds to no longer be probability, but log_odds. All occupancy octrees in octomap store the actual cell probability in logodds format. So you will see in the implementation where I now compare with getLogOdds() (which does no computation) to the log odds threshold. Its OK to bleed the concept of log odds into FCL octree here as it is part of the private implementation. Also log odds is an important part of how occupancy octree's work in octomap, as that is how the occupancy threshold in octomap already works. (Note that there is no corresponding free threshold in octomap, which is presumably the reason it was added to FCL at some point). Note that all the public APIs in FCL::octree continue to take probability, and those are correctly converted to logodds using octomap::logodds

3. Turning a probability into a logodds involves calling std::log, which isn't very efficient when doing many comparisons, as would be done in a distance query.

4. I will provide a test that would fail prior to this change.

Er...yeah...after I wrote all that, I still missed that, at the end of the day, I'd suggested log(0) was fine. D'oh!

[SeanCurtis-TRI]

No problem. :) I agree with both of your points. Next time I'll push back before review, and since this is reviewed, we'll let it roll as is with carefully curated commits.

Reviewable status: 12 of 13 files reviewed, 24 unresolved discussions (waiting on @c-andy-martin and @SeanCurtis-TRI)

[c-andy-martin]

I have responded to everything but the cleanup of the RSS tests and re-written history. I will get to that next week.

Reviewable status: 12 of 13 files reviewed, 9 unresolved discussions (waiting on @c-andy-martin and @SeanCurtis-TRI)

---

include/fcl/math/geometry.h, line 96 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: Given the work you did, it would be great to document the semantics of this function here. The things that jump out to me is:

1. This creates an orthonormal basis.
2. The first column of the input matrix is taken as the x-axis of the new basis.
3. While the x-axis doesn't have to be strictly unit length, it has to be "sufficiently" far from zero length to inspire confidence that it's a meaningful value. (In this case, greater than 1e-3).
4. The y- and z- axes will be arbitrarily oriented around the given x-axis.

Done

---

include/fcl/math/geometry-inl.h, line 649 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This is a very loose tolerance for the x-axis. Using this basis, you'll be introducing skew into transformed vertices relatively quickly. You still need to normalize this both for the output and prior to using it in the cross product.

Looking through eigen's source there are a couple places they use an assert threshold of 100 times the machine epsilon. I've switched to this instead, as it will scale w/ the precision of the data type. This assert is only meant as a way to detect the caller abusing the contract, which I have now documented explicitly that column zero must already be normalized. IOW, this method requires that the caller has already called .normalize() on column 0, meaning doing it again is just a waste of time. That is how this function is used in the few places it is called. The assert is just meant to catch someone not doing that.

---

include/fcl/math/geometry-inl.h, line 669 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This should be normalized just in case. Even with the correction of the x-axis (noted above), we can still introduce rounding error via a cross product that can be corrected by a re-normalization. Let's make the best quality basis possible.

there are other places in the same file where two unit vectors (already normalized) are crossed and the result is not normalized, for instance, fit3 around line 116. This does not seem necessary to me.

---

include/fcl/math/bv/RSS.h, line 47 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW According to the original paper (http://gamma.cs.unc.edu/SSV/ssv.pdf) RSS wouldbe "rectangle swept sphere" and not "rectangle sphere-swept". Again, as long as we're here, could we hit that as well?

Done

---

include/fcl/math/bv/RSS.h, line 58 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

btw I could I persuade you to fix this typo while you're here? "coulumn" vs "column".

Done

---

include/fcl/math/bv/RSS.h, line 59 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Also, this language that suggests there's the third column corresponds to the "shortest" axis is absurd because the RSS is a 2D rectangle embedded in 3D space and not a 3D rectangular solid like this language suggests. In fact, we expect the third column to be perpendicular to the rectangle. That's a much more interesting characterization.

Done

---

include/fcl/math/bv/RSS.h, line 63 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: You have to be careful with the definition of "minimum". Because the rectangle can be arbitrarily oriented in another frame, it may not be the minimum in that frame.

So, perhaps we need to elaborate further on the class:

RSS defines a rectangle swept sphere. The rectangle is defined in the frame T. It has one corner positioned at the origin of frame T. It lies on T's z = 0 plane. Its longest dimension extends in the +x direction, and its shorter dimension in the +y direction.

The RSS is then posed in another frame F. axis is the relative orientation between frame T and F and To is the position of T's origin in Frame F.

In the notation we've been stealing from Drake:

• To would be p_FoTo_F (the displacement vector from F's origin to T's origin, measured and expressed in frame F).
• axis would be R_FT (the relative orientation between frames F and T such that, e.g., p_FoQ_F = R_FT * p_FoQ_T).

Typically, FCL is lacking in rigor in talking about what the quantities are and how are they measured and expressed. I wouldn't remotely say we should change the names of axis or To (as they are part of public APIs that would cause all sorts of breakages). But we can certainly be more precise in the documentation.

Note: while this class can't know anything about the semantics of Frame F, it's probably still worth putting a note that frame F is, e.g., the frame of the geometry being bound.

I have cleaned all this up per your comments, and even made an aliases p_FoTo_F() and R_FT() and switched the setToFromCenter() and center() method implementations to use the new aliases for clarity.

---

include/fcl/math/bv/RSS.h, line 113 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit missing period.

Fixed

---

include/fcl/math/bv/utility-inl.h, line 843 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW: The RSS documentation declares correlation with rectangle measures and local axes. Looking just at this code, it isn't clear that this contract is maintained. However, if one reads the OBB documentation, one can see that OBB makes the same promise.

How do you feel about putting a note here indicating that RSS's documented requirement relies on OBB's documented requirement (vis a vis axes and extents)?

Done

---

include/fcl/math/bv/utility-inl.h, line 847 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: as long as you're changing this, let's swap bv1.To with bv1.center().

Done

---

test/test_fcl_octomap_distance.cpp, line 124 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This represents a huge change in tests. The CI compute budget given as an open source project is quite limited. And these tests aren't cheap. Given that we're simply generating a bunch of random transforms, it's not clear to me that doing more transforms per test execution is inherently better than just allowing the aggregate effect of many, many CI runs.

I agree that this is unnecessary and wasteful, removed. However, I believe the pseudo random number generator is always seeded the same, so the test will always either fail or succeed, and many CI runs is irrelevant.

---

test/test_fcl_octomap_distance.cpp, line 195 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Why are we testing transforms three times? I don't buy this. And if it's simply because I'm not seeing it, a quick doc explaining why it makes sense will alleviate future confusion.

My best guess is that you want to make sure you have at least one case where tf1 = tf2 = Identity but aren't confident that a single value in the center would align. Is that it? To be perfectly honest, I'm hard-pressed to believe that it has particular value. Both geometries being the identity will tell us the least about the code being exercised. If it works across random pairs of transforms, failure when they are both identities would require some kind of adversarial bug.

Concur. I stumbled on this as a way to get the tests to fail after finding bugs in RSS. However, after playing with it more today, I have figured out that I just need to narrow the extents to get the tree nearer the object on order to get the failing geometry. I have simply shrunk the extents, and now the test fails (without my RSS fixes).

---

test/test_fcl_octomap_distance.cpp, line 215 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This tolerance should be expressed in terms of the scalar type. See constants<S>::eps() or constants<S>::eps78() or constants<S>::eps12() (for a very loose tolerance).

Fixed.

---

test/test_fcl_octomap_distance.cpp, line 235 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

What does this code do? It runs, but nothing is asserted.

Umm...this was a merge failure. This commit was based on code from over a year ago, and when I fixed the merge issues, I screwed this up. Thanks for catching this, I've removed the dead code.

---

test/test_fcl_octomap_distance.cpp, line 240 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Could I talk you into deleting this console dump?

Done.

---

test/test_fcl_octomap_distance.cpp, line 251 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Do you mean for this to include "touching"? If not dist1 > 0.0.

This is a good question. I think not. Fixed.

---

test/test_fcl_utility.h, line 605 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I have one major concern about this change. The commit message suggests that this change is merely about pruning. I strongly suspect the resultant tree is actually different.

As I see it, these two for loops traverse overlapping spaces (the second for loop is a smaller, inset region). The outer loop "adds" occupancy to the whole map and the inner removes it from the inset region.

Using updateNode this is an "additive","subtractive" approach. Using setNodeValue() uses an overwrite approach. Furthermore, the mapping of the former bool values to probabilities creates different values in the cells than would be defined by the threshold values. It's impossible for me to believe that the variance in operators and operands produce an equivalent result.

Therefore, I believe this change needs to be well argued that it's a good change and it can't just be about pruning.

Agreed. Looking at this again, I'm not sure why I did this. It may have made the tests faster, but as you point out, other users of this method may rely on how it works to generate various levels of probability. I have removed the commit from the PR.

[c-andy-martin]

Reviewable status: 3 of 13 files reviewed, 6 unresolved discussions (waiting on @SeanCurtis-TRI)

---

test/test_fcl_math.cpp, line 141 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This function seems to be testing too much.

1. First we have a bunch of tests exercising the fit function.
2. Then we're testing the conversion.
3. Then distance
4. The setting the position of the RSS's rectangle.

This single test would benefit from being broken up into tests that more individually test those very different modes.

(Perhaps with a TODO listing the things that haven't been tested on RSS (e.g., overlap, contain, operator+, size, etc.)

Done

---

test/test_fcl_math.cpp, line 142 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: All of these would be better as EXPECT_EQ.

Done

---

test/test_fcl_math.cpp, line 147 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

All of these specific cases seem to be quite redundant. This doesn't look like a strategic test of the fit function. If I'm mistaken, some notes in the code indicating the purpose of this strategy will help educate future readers.

Done

---

test/test_fcl_math.cpp, line 259 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit Any reason you're including brand new, commented-out code?

Just following the pattern of the other code in the same file. Fixed.

[c-andy-martin]

I have responded to everything as of now.

Reviewable status: 3 of 13 files reviewed, 6 unresolved discussions (waiting on @SeanCurtis-TRI)

[SeanCurtis-TRI]

Thanks for your patience. End-of-quarter deadlines. You know how it is.

We're definitely in the home stretch. Mostly it's clean up with a larger topic (cheaply resolved) on the new API for RSS. Let me know if my essay isn't clear. :)

Reviewed 11 of 11 files at r2.
Reviewable status: all files reviewed, 8 unresolved discussions (waiting on @c-andy-martin)

---

include/fcl/math/geometry-inl.h, line 649 at r1 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

Looking through eigen's source there are a couple places they use an assert threshold of 100 times the machine epsilon. I've switched to this instead, as it will scale w/ the precision of the data type. This assert is only meant as a way to detect the caller abusing the contract, which I have now documented explicitly that column zero must already be normalized. IOW, this method requires that the caller has already called .normalize() on column 0, meaning doing it again is just a waste of time. That is how this function is used in the few places it is called. The assert is just meant to catch someone not doing that.

I suspect Eigen is using a looser tolerance because it has to account for multiple rotation matrix multiplications. With each multiplication, we drift away from being truly orthonormal. So, it has a large tolerance to account for that inevitable drift (as opposed to orthonormalizing after each multiplication).

In this case, when we're creating a basis from scratch, we should have the highest possible standards.

Really, we should simply normalize it and indicate that we're going to do so. The only requirement is that the vector is long enough that we don't think they're trying to create a basis from garbage.

---

include/fcl/math/geometry-inl.h, line 669 at r1 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

there are other places in the same file where two unit vectors (already normalized) are crossed and the result is not normalized, for instance, fit3 around line 116. This does not seem necessary to me.

I'm going to beat this horse a bit more in the hopes that my ability to persuade you matches my conviction. :)

1. Normalizing a vector will get a vector within epsilon from having unit length.
2. You're guaranteed that col(1) is unit length (up to machine precision) by calling unitOrthogonal().
3. That means col(0) could be up to two orders of magnitude closer to unit length than col(1).
4. col(0) will infect col(2).
5. Your point about crossing two unit vectors producing a not necessarily totally unit vector is true. That simply means that if we care about creating a high-quality basis, we should be normalizing every column (including the cross product of col(0) and col(1)).
6. You're only validating in debug builds which means people could be creating bad bases all the time and not be aware of it. It'll have an accumulative negative impact that they'll only ever find out if they run FCL in debug. How often is that going to happen? Also, it may be that the code calling this function has its own debug behavior that pre-normalizes col(0). The function should be able to stand on its own two legs and state something definitive about the basis that gets returned.

Is that persuasive?

---

include/fcl/math/geometry-inl.h, line 653 at r2 (raw file):

  // Axis column 0 should be passed already normalized.
  assert(std::abs(1.0 - axis.col(0).norm()) < constants<S>::eps_78());
  axis.col(1).noalias() = axis.col(0).unitOrthogonal();

BTW Great use of the API. I hadn't know about this method. That gets filed away in my brain!

---

include/fcl/math/bv/RSS.h, line 54 at r2 (raw file):
nit You're teaching me all sorts of things. I didn't know about. #axis. That's brilliant.

However, I'm not sure I buy the formatting of thing / #name. The slash doesn't communicate much to me. I don't remember seeing it before and the rendered doxygen isn't particularly compelling.

Perhaps this might be more clear.

R_FT (the #axis field) is the relative orientation.... and p_FoToF (the #To field) is the position of ...

---

include/fcl/math/bv/RSS.h, line 73 at r2 (raw file):

  /// @brief Monogram notation alias of #axis.
  inline Matrix3<S>& R_FT() { return axis; }

I'm going to argue against the methods R_FT() and p_FoTo_F(). Stick with me, because the argument is subtle, but, it's worthwhile. The more I've worked with this notation, the more strongly I've become convinced of what I'm about to say.

tl;dr: we shouldn't use monogram notation in public APIs, only in documentation, parameter names, and non-public members.

Argument:

Introducing names of frames in a public API pollutes the symbols in the callers domain. We'll use this as a concrete example. In this case, RSS has defined two symbols: F and T. The definitions are helpful and necessary so it can accurately describe what it is and how it relates to inputs.

Here's where things get bad.

Case 1: The caller code knows that F is the world frame W. They end up with code like this:

// Some point Q in world space, measured and expressed in the
// world frame.
Vector3<S> p_WoQ_W{ ... };
// I've defined an RSS such that F is W. Imagine
// appropriate actions to set axis and To.
RSS<S> my_rss_W;
// I want to reexpress the position vector between Wo and Q
// in the RSS's frame. But now I've a quantity on the left in
// terms of T and W and on the right I have F, T, and W. It
// requires a comment explaining that we know F  to be W.
const Matrix3<S>& R_TW = my_rss_W.R_FT().inverse();
Vector3S<T> p_WoQ_T = R_TW * p_WoQ_W;

Case 2: The caller has already used symbols F or T to mean something else.

For example, imagine an operation between an RSS and a triangle. In my code, I may think of the triangle being conveniently defined in the triangle frame T. But references to RSS::R_FT() will conflict with that symbol.

Conclusion:

For that reason, I advocate different names.

• Instead of Transform<S> X_AB(), it's Transform<S> pose().
• Instead of Matrix3<S> R_AB() it's Matrix3<S> orientation().
• Instead of Vector3<S> p_AoBo_A(), it's Vector3<S> position() (or origin()).

And then the documentation carries the burden of saying that it is, for example, the orientation of the RSS's frame T in the reference frame F.

And then the code example above goes from:

// Old line - introduced symbol F.
const Matrix3<S>& R_TW = my_rss_W.R_FT().inverse();
// New line - all symbols are consistent with the *local*
// conventions of the caller.
const Matrix3<S>& R_TW = my_rss_W.orientation().inverse();

---

include/fcl/math/bv/RSS.h, line 82 at r2 (raw file):

  /// Note this is the translation in frame F to the origin of frame T.
  /// The rectangle's minimum corner in frame T is at the origin of frame T.
  /// At one point in the past, some code treated #To as the center

nit: Don't need # on To. We're already at the documentation for To.

---

include/fcl/math/bv/RSS.h, line 83 at r2 (raw file):
BTW: The "At one point in the past..." clause is interesting. I think it's an interesting point to be made (as clearly, even FCL couldn't protect itself from interpreting it incorrectly). However, how much of that is due to the fact that it was never reasonably documented? I'd suggest one of two things:

1. This last sentence isn't necessary because the previous sentence is so clear.
2. If you feel strongly about keeping it, I'd express it, not in terms of past mistakes, but as a warning against future mistakes. E.g.,

It should never be interpreted as the center of the rectangle or as the maximum corner (in frame T).

---

include/fcl/math/bv/RSS.h, line 136 at r2 (raw file):
nit: while you're here, you can extend this comment to match the rest of the documentation:

The RSS center C, measured and expressed in frame F: p_FoC_F.

---

include/fcl/math/bv/RSS-inl.h, line 420 at r2 (raw file):

  Vector3<S> p_ToCenter_T;
  p_ToCenter_T << l[0] * 0.5, l[1] * 0.5, 0.0;
  return p_FoTo_F() + R_FT() * p_ToCenter_T;

BTW: It's so nice to see the frames all line up nicely. :)

---

include/fcl/math/bv/utility-inl.h, line 849 at r2 (raw file):

    bv2.axis.noalias() = tf1.linear() * bv1.axis;

    // Set longest rectangle side for RSS to longest dimension of OBB

nit: The next three comments are all missing periods at the end.

---

test/test_fcl_math.cpp, line 305 at r2 (raw file):

}

GTEST_TEST(FCL_MATH, rss)

BTW My personal preference would be for a separate test case (GTEST_TEST) for each aspect of the RSS. If a problem ever comes up with one aspect, we'll keep running the tests for all aspects. But I'm not going to require a change from you. What you've done is absolutely an improvement of what was here before.

---

test/test_fcl_octomap_distance.cpp, line 124 at r1 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

I agree that this is unnecessary and wasteful, removed. However, I believe the pseudo random number generator is always seeded the same, so the test will always either fail or succeed, and many CI runs is irrelevant.

Ah....I'd missed the seed. :-( I've always hated unit tests that just throw random values at the code. Fuzzy testing has a place, but it requires a meaningful infrastructure to capture the failure cases for reproducibility. Without that, random crap isn't justified. Still, the sin predates us....

[c-andy-martin]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SeanCurtis-TRI)

---

include/fcl/math/geometry-inl.h, line 669 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm going to beat this horse a bit more in the hopes that my ability to persuade you matches my conviction. :)

1. Normalizing a vector will get a vector within epsilon from having unit length.
2. You're guaranteed that col(1) is unit length (up to machine precision) by calling unitOrthogonal().
3. That means col(0) could be up to two orders of magnitude closer to unit length than col(1).
4. col(0) will infect col(2).
5. Your point about crossing two unit vectors producing a not necessarily totally unit vector is true. That simply means that if we care about creating a high-quality basis, we should be normalizing every column (including the cross product of col(0) and col(1)).
6. You're only validating in debug builds which means people could be creating bad bases all the time and not be aware of it. It'll have an accumulative negative impact that they'll only ever find out if they run FCL in debug. How often is that going to happen? Also, it may be that the code calling this function has its own debug behavior that pre-normalizes col(0). The function should be able to stand on its own two legs and state something definitive about the basis that gets returned.

Is that persuasive?

What I have done is made the basis vector be an input and then calculate the best possible axes. This way we do not pay to normalize the input twice. I have removed normalizing the input from the callers.

---

include/fcl/math/bv/RSS.h, line 73 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm going to argue against the methods R_FT() and p_FoTo_F(). Stick with me, because the argument is subtle, but, it's worthwhile. The more I've worked with this notation, the more strongly I've become convinced of what I'm about to say.

tl;dr: we shouldn't use monogram notation in public APIs, only in documentation, parameter names, and non-public members.

Argument:

Introducing names of frames in a public API pollutes the symbols in the callers domain. We'll use this as a concrete example. In this case, RSS has defined two symbols: F and T. The definitions are helpful and necessary so it can accurately describe what it is and how it relates to inputs.

Here's where things get bad.

Case 1: The caller code knows that F is the world frame W. They end up with code like this:

// Some point Q in world space, measured and expressed in the
// world frame.
Vector3<S> p_WoQ_W{ ... };
// I've defined an RSS such that F is W. Imagine
// appropriate actions to set axis and To.
RSS<S> my_rss_W;
// I want to reexpress the position vector between Wo and Q
// in the RSS's frame. But now I've a quantity on the left in
// terms of T and W and on the right I have F, T, and W. It
// requires a comment explaining that we know F  to be W.
const Matrix3<S>& R_TW = my_rss_W.R_FT().inverse();
Vector3S<T> p_WoQ_T = R_TW * p_WoQ_W;

Case 2: The caller has already used symbols F or T to mean something else.

For example, imagine an operation between an RSS and a triangle. In my code, I may think of the triangle being conveniently defined in the triangle frame T. But references to RSS::R_FT() will conflict with that symbol.

Conclusion:

For that reason, I advocate different names.

• Instead of Transform<S> X_AB(), it's Transform<S> pose().
• Instead of Matrix3<S> R_AB() it's Matrix3<S> orientation().
• Instead of Vector3<S> p_AoBo_A(), it's Vector3<S> position() (or origin()).

And then the documentation carries the burden of saying that it is, for example, the orientation of the RSS's frame T in the reference frame F.

And then the code example above goes from:

// Old line - introduced symbol F.
const Matrix3<S>& R_TW = my_rss_W.R_FT().inverse();
// New line - all symbols are consistent with the *local*
// conventions of the caller.
const Matrix3<S>& R_TW = my_rss_W.orientation().inverse();

I have decided instead to make the aliases private. They were only being used internally now anyway, and the names make good sense. I don't want to introduce new public aliases just yet, unless you feel strongly I need to add pose, orientation, origin, etc.

---

include/fcl/math/bv/RSS.h, line 83 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW: The "At one point in the past..." clause is interesting. I think it's an interesting point to be made (as clearly, even FCL couldn't protect itself from interpreting it incorrectly). However, how much of that is due to the fact that it was never reasonably documented? I'd suggest one of two things:

1. This last sentence isn't necessary because the previous sentence is so clear.
2. If you feel strongly about keeping it, I'd express it, not in terms of past mistakes, but as a warning against future mistakes. E.g.,

It should never be interpreted as the center of the rectangle or as the maximum corner (in frame T).

It seems like less is more here. I'll remove it.

[c-andy-martin]

No worries. I've been away some for the 4th holiday. I think I have responded to everything.

[SeanCurtis-TRI]

Phenomenal! I've made three passing suggestions on documentation language but will leave it up to you.

+@sherm1 for platform.

Reviewed 7 of 7 files at r3.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

---

include/fcl/math/geometry.h, line 96 at r3 (raw file):

/// @brief compute orthogonal coordinate system basis with given x-axis.
///
/// @param[in]  v        Principal axis direction. Must be non-zero.

BTW: The choice of the word "principal" here can lead to confusion. Maybe better to explicitly refer to the x-axis here as well?

---

include/fcl/math/geometry.h, line 96 at r3 (raw file):
BTW: Must be non-zero as a requirement is strictly true. But I believe we can provide more guidance to the user.

Must have non-zero length. For best results, length should be much larger than constants::epsilon12().

Or some such thing.

---

include/fcl/math/geometry.h, line 98 at r3 (raw file):

/// @param[in]  v        Principal axis direction. Must be non-zero.
/// @param[out] axis_ptr Must point to a valid Matrix3<S>.
///                      Column 0 is v normalized.

BTW I'm not sure how much of this is knowing the history and how much is a linguistic subtlety. But the choice of present tense ("is" and "are") caused me a mild amount of confusion. I wasn't sure if there were expectations about the values in the matrix. Perhaps "Column 0 will be v normalized", etc.?

---

include/fcl/math/bv/RSS.h, line 73 at r2 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

I have decided instead to make the aliases private. They were only being used internally now anyway, and the names make good sense. I don't want to introduce new public aliases just yet, unless you feel strongly I need to add pose, orientation, origin, etc.

Perfect!

[SeanCurtis-TRI]

Reviewed 3 of 3 files at r4.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

[SeanCurtis-TRI]

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

---

include/fcl/math/geometry.h, line 94 at r4 (raw file):

                   Transform3<S>& tf);

/// @brief compute orthogonal coordinate system basis with given x-axis.

BTW This has evolved so gradually to this point, that I've missed the elephant in the room. With the fact that you're passing in x_axis explicitly, there's no longer any need to do this C++03 style. We can bump it up to represent modern C++:

Matrix3<S> generateCoordinateSystem(const Vetctor3<S>& x_axis);

They've worked so hard to gives us the ever-so-convenient copy elision, let's make use of it.

[c-andy-martin]

---

include/fcl/math/geometry.h, line 94 at r4 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This has evolved so gradually to this point, that I've missed the elephant in the room. With the fact that you're passing in x_axis explicitly, there's no longer any need to do this C++03 style. We can bump it up to represent modern C++:

Matrix3<S> generateCoordinateSystem(const Vetctor3<S>& x_axis);

They've worked so hard to gives us the ever-so-convenient copy elision, let's make use of it.

I agree, I will implement this today.

[c-andy-martin]

Reviewable status: 8 of 13 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI and @sherm1)

---

include/fcl/math/geometry.h, line 94 at r4 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

I agree, I will implement this today.

Done.

[SeanCurtis-TRI]

The CI failure is mystifying -- tens of thousands of weirdo lines with no apparent error. I retriggered it to see what comes of it. Certainly, everything in this revision makes sense and seems reasonable.

Reviewed 5 of 5 files at r5.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

[SeanCurtis-TRI]

Well, retriggering did the trick on one of the failures. Let's see what it does for the others.

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

[sherm1]

Checkpoint. Just the tests remaining to review.

Reviewed 3 of 12 files at r1, 1 of 11 files at r2, 1 of 7 files at r3, 5 of 5 files at r5.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on @c-andy-martin)

---

include/fcl/math/geometry.h, line 100 at r5 (raw file):

///                constants<S>::eps_12().
/// @return The coordinate system in a Matrix3<S>.
///         Column 0 will be the normalied x-axis.

nit: normalied -> normalized

---

include/fcl/math/geometry-inl.h, line 647 at r5 (raw file):

template <typename S>
FCL_EXPORT
Matrix3<S> generateCoordinateSystem(const Vector3<S>& x_axis)

BTW oh it is so nice to see all that code get deleted!!

---

include/fcl/math/bv/RSS.h, line 47 at r5 (raw file):

{

/// @brief A class for rectangle swept sphere bounding volume

nit: missing period

---

include/fcl/math/bv/RSS.h, line 138 at r5 (raw file):

private:
  /// @brief Internal monogram notation alias of #axis.

nit: doxygen skips private members so the /// @brief is misleading. Consider just // Internal monogram notation alias of #axis. here and below.

---

include/fcl/math/bv/RSS.h, line 139 at r5 (raw file):

private:
  /// @brief Internal monogram notation alias of #axis.
  inline Matrix3<S>& R_FT() { return axis; }

nit inline (here and below) -- all methods defined in a class header have inline linkage so this is a no-op here. Leaving it in is misleading since a reader will naturally assume it means something. In particular it does not force the compiler to inline the code.

---

include/fcl/math/bv/utility-inl.h, line 318 at r5 (raw file):

  const Vector3<S>& p2 = ps[1];
  const Vector3<S> p1p2 = p1 - p2;
  S len_p1p2 = p1p2.norm();

nit: const

---

include/fcl/math/bv/utility-inl.h, line 847 at r5 (raw file):

    // Set shortest rectangle side for RSS to next-longest dimension of OBB.
    bv2.l[1] = 2 * (bv1.extent[1]);
    // Set radius for RSS to the smallest dimesion of OBB.

nit dimesion -> dimension

[sherm1]

It's really nice to see such a substantive contribution to FCL! Thanks, Andy. (And thanks for the extensive and productive review, Sean.)
Platform pending a few minor comments. CI looks clean now.

Reviewed 2 of 11 files at r2, 1 of 7 files at r3.
Reviewable status: all files reviewed, 9 unresolved discussions (waiting on @c-andy-martin)

---

test/test_fcl_math.cpp, line 136 at r5 (raw file):

template <typename S>
void test_rss_fit()

minor: please add a function comment explaining what this is testing. (same for the other tests please)

---

test/test_fcl_math.cpp, line 138 at r5 (raw file):

void test_rss_fit()
{
  S epsilon = constants<S>::eps_78();

nit const (also for the other epsilons below)

---

test/test_fcl_octomap_distance.cpp, line 185 at r5 (raw file):

  m1->endModel();

  OcTree<S>* tree = new OcTree<S>(std::shared_ptr<octomap::OcTree>(test::generateOcTree(resolution)));

btw this line is too long in case you want to tidy up while you're here

---

test/test_fcl_octomap_distance.cpp, line 207 at r5 (raw file):

    S dist1b = std::numeric_limits<S>::max();
    DefaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
    DefaultDistanceFunction(&obj2, &obj1, &cdata1b, dist1b);

minor: I don't understand why you are doing this twice now. Can you add some comments clarifying the purpose of this test?

---

test/test_fcl_octomap_distance.cpp, line 216 at r5 (raw file):

      boxes[j]->setTransform(tf2 * boxes[j]->getTransform());

    DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>();

btw line length

---

test/test_fcl_octomap_distance.cpp, line 234 at r5 (raw file):

    // since the problem is ill-posed (i.e. the nearest points may differ widely
    // for slightly different geometries)
    Vector3<S> nearestPointDistance = cdata.result.nearest_points[0] - cdata.result.nearest_points[1];

btw line length

[c-andy-martin]

Reviewable status: 8 of 13 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI and @sherm1)

---

include/fcl/math/bv/RSS.h, line 138 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: doxygen skips private members so the /// @brief is misleading. Consider just // Internal monogram notation alias of #axis. here and below.

yeah, we moved the code and I forgot that, good catch!

[c-andy-martin]

OK, thanks @sherm1 for the review, I have addressed all your comments and everything should be taken care of.

[SeanCurtis-TRI]

Reviewed 5 of 5 files at r6.
Reviewable status: complete! all files reviewed, all discussions resolved

[sherm1]

Reviewed 5 of 5 files at r6.
Reviewable status: complete! all files reviewed, all discussions resolved

[SeanCurtis-TRI]

Huzzah! Thanks a million, Andy. Consider this MERGED!

Reviewable status: complete! all files reviewed, all discussions resolved