Pad bounding box volumes to avoid roundoff issues

[tehbelinda]

This change is 

[tehbelinda]

+@DamrongGuoy for feature review please

[DamrongGuoy]

with minor requests.

Reviewable status: 7 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @DamrongGuoy and @tehbelinda)

---

geometry/proximity/bounding_volume_hierarchy.cc, line 66 at r1 (raw file):

}

void Aabb::StretchBoundary() {

BTW, the name StretchBoundary() doesn't sound appealing to me. On the other hand, I don't have a better name either. It's fine to keep it.

---

geometry/proximity/bounding_volume_hierarchy.cc, line 68 at r1 (raw file):

void Aabb::StretchBoundary() {
  const double max_dim = center_.cwiseAbs().maxCoeff();
  const double max_rad = half_width_.maxCoeff();

BTW, would max_position and max_half_width sound better than max_dim and max_rad?

Otherwise, please use full name max_radius instead of max_rad. (GSG: Always prefer long, human-readable names https://drake.mit.edu/styleguide/cppguide.html#General_Naming_Rules)

---

geometry/proximity/bounding_volume_hierarchy.cc, line 72 at r1 (raw file):

  const double incr =
      std::max(scale * std::numeric_limits<double>::epsilon(), kTolerance);
  half_width_ += Vector3<double>::Ones() * incr;

Minor. Change Vector3<double>::Ones() * incr; to Vector3<double>::Constant(incr); please.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 24 at r1 (raw file):

// Default tolerance for double precision.
const double kDefaultTolerance = 2e-14;

Do we assume this kDefaultTolerance to be the same as the private: Aabb::kTolerance ? If so, please add comments like this:

// Default tolerance for double precision.
// Must use the same value as Aabb::kTolerance.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 286 at r1 (raw file):

  Aabb zero_aabb = Aabb(Vector3d(3, -4, 1.3), Vector3d(0, 0, 0));
  const double zero_volume = zero_aabb.CalcVolume();
  EXPECT_NEAR(zero_aabb.CalcVolume(), 0, kDefaultTolerance * 100);

Minor. Please reduce the tolerance for EXPECT_NEAR.

I would think the half_width of the zero-volume box would be stretched from (0,0,0) to (2e-14, 2e-14, 2e-14), so its volume should be 64e-42.

Therefore, kDefaultTolerance should be enough, no need for 100x.

Please let me know if I miscalculated.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 308 at r1 (raw file):

  // dimension or position in the frame.
  const double stretch_tolerance = 300 * std::numeric_limits<double>::epsilon();
  EXPECT_GT(stretch_tolerance, kDefaultTolerance);

Minor. Would ASSERT_GT be better than EXPECT_GT here? This line checks the code of the unit test, not the code of Aabb. Please let me know if I misunderstood.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 309 at r1 (raw file):

  const double stretch_tolerance = 300 * std::numeric_limits<double>::epsilon();
  EXPECT_GT(stretch_tolerance, kDefaultTolerance);
  aabb = Aabb(Vector3d(-100, 50, 100), Vector3d(120, 250, 300));

Minor. Would it help readers if we distinguish this line from the three lines below with comments like this?

// half_width.z is the max.
 aabb = Aabb(Vector3d(-100, 50, 100), Vector3d(120, 250, 300));

// |center.x| is the max.
aabb = Aabb(Vector3d(-300, 50, 100), Vector3d(120, 250, 200));

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 311 at r1 (raw file):

  aabb = Aabb(Vector3d(-100, 50, 100), Vector3d(120, 250, 300));
  stretched = Vector3d(120, 250, 300).array() + stretch_tolerance;
  EXPECT_TRUE(CompareMatrices(aabb.half_width(), stretched));

BTW, perhaps we want to mention that we expect the two Vector3d to be equal down to the last bits. That's why we use CompareMatrices's default tolerance, which is zero. Something like this?

// Expect the two Vector3d to be exactly equal. 

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 314 at r1 (raw file):

  aabb = Aabb(Vector3d(-300, 50, 100), Vector3d(120, 250, 200));
  stretched = Vector3d(120, 250, 200).array() + stretch_tolerance;
  EXPECT_TRUE(CompareMatrices(aabb.half_width(), stretched));

// Expect the two Vector3d to be exactly equal. 

[DamrongGuoy]

Reviewed 3 of 3 files at r1.
Reviewable status: 7 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @tehbelinda)

[tehbelinda]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @DamrongGuoy)

---

geometry/proximity/bounding_volume_hierarchy.cc, line 66 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, the name StretchBoundary() doesn't sound appealing to me. On the other hand, I don't have a better name either. It's fine to keep it.

Hmm, what about AddBoundaryTolerance?

---

geometry/proximity/bounding_volume_hierarchy.cc, line 68 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, would max_position and max_half_width sound better than max_dim and max_rad?

Otherwise, please use full name max_radius instead of max_rad. (GSG: Always prefer long, human-readable names https://drake.mit.edu/styleguide/cppguide.html#General_Naming_Rules)

yes, those sound better to me too

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 24 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

Do we assume this kDefaultTolerance to be the same as the private: Aabb::kTolerance ? If so, please add comments like this:

// Default tolerance for double precision.
// Must use the same value as Aabb::kTolerance.

Yes, done.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 286 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

Minor. Please reduce the tolerance for EXPECT_NEAR.

I would think the half_width of the zero-volume box would be stretched from (0,0,0) to (2e-14, 2e-14, 2e-14), so its volume should be 64e-42.

Therefore, kDefaultTolerance should be enough, no need for 100x.

Please let me know if I miscalculated.

Ah yes that's correct for the zero case, but not for the other it seems?

The difference between volume and 21.6 is 1.2398970739013748e-12, which exceeds kDefaultTolerance * 10, where
volume evaluates to 21.600000000001241,
21.6 evaluates to 21.600000000000001, and
kDefaultTolerance * 10 evaluates to 2.0000000000000001e-13.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 308 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

Minor. Would ASSERT_GT be better than EXPECT_GT here? This line checks the code of the unit test, not the code of Aabb. Please let me know if I misunderstood.

Yes that's correct, changed

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 309 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

Minor. Would it help readers if we distinguish this line from the three lines below with comments like this?

// half_width.z is the max.
 aabb = Aabb(Vector3d(-100, 50, 100), Vector3d(120, 250, 300));

// |center.x| is the max.
aabb = Aabb(Vector3d(-300, 50, 100), Vector3d(120, 250, 200));

Done.

[SeanCurtis-TRI]

+@SeanCurtis-TRI for platform review

Reviewed 2 of 2 files at r2.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform) (waiting on @DamrongGuoy and @tehbelinda)

---

geometry/proximity/bounding_volume_hierarchy.h, line 75 at r2 (raw file):

  void StretchBoundary();

  // Default tolerance for double precision.

nit: Worth noting that this is more than "default tolerance" this is the minimum amount of padding to add, regardless of box size or position.

---

geometry/proximity/bounding_volume_hierarchy.cc, line 66 at r1 (raw file):

Previously, tehbelinda (Bel) wrote…

Hmm, what about AddBoundaryTolerance?

Perhaps "pad" instead of "stretch"?

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 24 at r1 (raw file):

Previously, tehbelinda (Bel) wrote…

Yes, done.

btw Alternatively, you could declare a friend class to Aabb and have it directly grab the value to avoid implicit coordination.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 286 at r1 (raw file):

Previously, tehbelinda (Bel) wrote…

Ah yes that's correct for the zero case, but not for the other it seems?

The difference between volume and 21.6 is 1.2398970739013748e-12, which exceeds kDefaultTolerance * 10, where
volume evaluates to 21.600000000001241,
21.6 evaluates to 21.600000000000001, and
kDefaultTolerance * 10 evaluates to 2.0000000000000001e-13.

In the other, you've gone from lengths: (a, b, c) to (A, B, C) such that A = a + ε. So, the volume goes from 8abc --> 8((a + ε)*(b + ε)*(c+ε)) --> 8[abc + (ab + bc + ac)ε + (a + b + c)ε² + ε³]. So, the difference between the unpadded volume and the padded volume is an increase of:
(ab + bc + ac)ε + (a + b + c)ε² + ε³. The error is dominated by the first term
8(ab + bc + ac)ε. For the values of a, b, and c, I get that at a value of roughly 66ε. Related to my other note, this would be the justification of a factor of about 100 (I'd argue that you could cap it out at 70).

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 283 at r2 (raw file):

  Aabb aabb = Aabb(Vector3d(-1, 2, 1), Vector3d(2, 0.5, 2.7));
  const double volume = aabb.CalcVolume();
  EXPECT_NEAR(volume, 21.6, kDefaultTolerance * 100);

nit: A note on this scale factor of 100.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 295 at r2 (raw file):

  Aabb aabb = Aabb(Vector3d(-1, 2, 1), Vector3d(2, 0.5, 2.7));
  EXPECT_TRUE(CompareMatrices(aabb.upper(), Vector3d(1, 2.5, 3.7),
                              2 * kDefaultTolerance));

BTW A factor of 2 is surprising. Upper is center + half size, and only one "pad" has been added to that quantity. Why 2?

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 311 at r2 (raw file):

  ASSERT_GT(stretch_tolerance, kDefaultTolerance);
  // Max is set from half_width.z.
  aabb = Aabb(Vector3d(-100, 50, 100), Vector3d(120, 250, 300));

nit: These tests are combining big translations and big scales. It would be good to do cases where only one of those can be responsible for the increased padding so that we know that each directly contributes.

Although, this is mitigated by the fact that you're doing exact bit equivalency on the box sizes. I'm open to that argument.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 313 at r2 (raw file):

  aabb = Aabb(Vector3d(-100, 50, 100), Vector3d(120, 250, 300));
  stretched = Vector3d(120, 250, 300).array() + stretch_tolerance;
  // Expect the two Vector3d to be exactly equal down to the last bits.

BTW "...down to the last bits" seems an odd quasi-idiom. "...down to the last bit." feels more typical.

[tehbelinda]

---

geometry/proximity/bounding_volume_hierarchy.cc, line 66 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Perhaps "pad" instead of "stretch"?

Oh yeah I like that, PadBoundary()

[tehbelinda]

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 295 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW A factor of 2 is surprising. Upper is center + half size, and only one "pad" has been added to that quantity. Why 2?

Okay 2 might be overkill, but there's still a slight difference. Actually, the upper() case is fine, it's just the lower() case. Is it possible that there's some loss due to the introduction of the negative bit??

Value of: CompareMatrices(aabb.lower(), Vector3d(-3, 1.5, -1.7), AabbTester::kTolerance)
  Actual: false (Values at (2, 0) exceed tolerance: -1.7000000000000202 vs. -1.7, diff = 2.0206059048177849e-14, tolerance = 2e-14
m1 =
  -3.00000000000002
   1.49999999999998
-1.7000000000000202
m2 =
  -3
 1.5
-1.7
delta=
-1.9984014443252818e-14
-1.9984014443252818e-14
-2.0206059048177849e-14)

[SeanCurtis-TRI]

Reviewable status: 5 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform) (waiting on @tehbelinda)

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 295 at r2 (raw file):

Previously, tehbelinda (Bel) wrote…

Okay 2 might be overkill, but there's still a slight difference. Actually, the upper() case is fine, it's just the lower() case. Is it possible that there's some loss due to the introduction of the negative bit??

Value of: CompareMatrices(aabb.lower(), Vector3d(-3, 1.5, -1.7), AabbTester::kTolerance)
  Actual: false (Values at (2, 0) exceed tolerance: -1.7000000000000202 vs. -1.7, diff = 2.0206059048177849e-14, tolerance = 2e-14
m1 =
  -3.00000000000002
   1.49999999999998
-1.7000000000000202
m2 =
  -3
 1.5
-1.7
delta=
-1.9984014443252818e-14
-1.9984014443252818e-14
-2.0206059048177849e-14)

So, the threshold you really want is kDefaultTolerance + std::numeric_limits<double>::epsilon(). That is, essentially, any of the rounding error you're going to get in the add/subtract.

[tehbelinda]

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform) (waiting on @DamrongGuoy and @SeanCurtis-TRI)

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 24 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

btw Alternatively, you could declare a friend class to Aabb and have it directly grab the value to avoid implicit coordination.

Done

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 295 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

So, the threshold you really want is kDefaultTolerance + std::numeric_limits<double>::epsilon(). That is, essentially, any of the rounding error you're going to get in the add/subtract.

Ahh got it, thanks

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 311 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: These tests are combining big translations and big scales. It would be good to do cases where only one of those can be responsible for the increased padding so that we know that each directly contributes.

Although, this is mitigated by the fact that you're doing exact bit equivalency on the box sizes. I'm open to that argument.

Sure that sounds good to me anyway

[DamrongGuoy]

Reviewed 3 of 3 files at r3.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform)

[SeanCurtis-TRI]

+@soonho-tri for platform review, please.

Reviewed 2 of 3 files at r3.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee soonho-tri(platform) (waiting on @soonho-tri and @tehbelinda)

---

geometry/proximity/bounding_volume_hierarchy.h, line 74 at r3 (raw file):

  // A very large box, or a box that is very far from the origin, must be
  // padded more than a small one at the origin.
  void PadBoundary();

BTW Generally, this looks good. I'm just not a big fan of the word "boundary". It doesn't seem that "boundary" is a word that would normally appear in the context of AABBs.

However, this is private and I don't have a ready, simple alternative so I'm content to simply expressing mild dissatisfaction. No action required unless you feel the same.

---

geometry/proximity/test/bounding_volume_hierarchy_test.cc, line 24 at r3 (raw file):

// Friend class for accessing AAbb's protected/private functionality.
class AabbTester {

BTW If you made AabbTester subclass ::testing::Test and changed the tests to TEST_F, then each of those tests could reference Aabb::kTolerance directly. :)

[SeanCurtis-TRI]

-@soonho-tri Oops...I did platform review. :)

Reviewable status: 2 unresolved discussions (waiting on @tehbelinda)

[tehbelinda]

---

geometry/proximity/bounding_volume_hierarchy.h, line 74 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Generally, this looks good. I'm just not a big fan of the word "boundary". It doesn't seem that "boundary" is a word that would normally appear in the context of AABBs.

However, this is private and I don't have a ready, simple alternative so I'm content to simply expressing mild dissatisfaction. No action required unless you feel the same.

PadHalfWidth?

[DamrongGuoy]

Reviewed 1 of 1 files at r4.
Reviewable status: complete! all discussions resolved, LGTM from assignees SeanCurtis-TRI(platform),DamrongGuoy

[SeanCurtis-TRI]

Reviewed 1 of 1 files at r4.
Reviewable status: complete! all discussions resolved, LGTM from assignees SeanCurtis-TRI(platform),DamrongGuoy

---

geometry/proximity/bounding_volume_hierarchy.h, line 74 at r3 (raw file):

Previously, tehbelinda (Bel) wrote…

PadHalfWidth?

Meh.

[tehbelinda]

---

geometry/proximity/bounding_volume_hierarchy.h, line 74 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Meh.

ok I won't bother then :)