Option to specify AABBs when calculating pairwise intersections for GCS.

[cohnt]

Per our discussion in #21593, this adds an additional argument to GeodesicConvexity::CalcPairwiseIntersections that takes in axis-aligned bounding boxes. If the user computes the AABBs themselves, they can be reused, saving time on recomputation.

+@sadraddini for feature review, please

---

This change is 

[cohnt]

Looks like the test failure on mac is an instance of #21715? I'll try rerunning

@drake-jenkins-bot mac-arm-ventura-clang-bazel-experimental-release please

[cohnt]

Now that #21715 is closed, I'll rerun the mac build.

@drake-jenkins-bot mac-arm-ventura-clang-bazel-experimental-release please

[sadraddini]

I'll review it once it passes the CI

[sadraddini]

Checkpoint for small comment.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/geodesic_convexity.h line 195 at r1 (raw file):

@param maybe_bboxes is a vector of Hyperrectangles, allowing the user to
manually pass in the AABBs of each set in `convex_sets` to avoid recomputation.
This argument is only used if `preprocess_bbox` is `true`, and the function does

Isn't it better to have a CalcPairwiseIntersections overload with boxes? It is confusing to have both this and preprocess_bbox.

Applies to the overload above as well. You would have 4 overloads.

[cohnt]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/geodesic_convexity.h line 195 at r1 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

Isn't it better to have a CalcPairwiseIntersections overload with boxes? It is confusing to have both this and preprocess_bbox.

Applies to the overload above as well. You would have 4 overloads.

I'm concerned that adding more overloads would lead to more code reuse, since the bounding box check is rather intermixed with the MathematicalProgram intersection check.

The goal is to support the following three usage patterns:

• User wants pairwise intersections, does not want to preprocess using bounding boxes (maybe most of the regions overlap)
• User wants pairwise intersections, does want to preprocess using bounding boxes
• User already computed bounding boxes (so absolutely should use them to preprocess)

Right now, we technically support a user providing bounding boxes, but setting preprocess_bbox to false. Is this where the confusion stems from? We could explicitly disallow it, or just ignore preprocess_bbox if a user provides bounding boxes. We can also update the documentation to be a bit more clear if that helps.

[sadraddini]

Some more discussion about the API

Reviewable status: 3 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/geodesic_convexity.h line 195 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

I'm concerned that adding more overloads would lead to more code reuse, since the bounding box check is rather intermixed with the MathematicalProgram intersection check.

The goal is to support the following three usage patterns:

• User wants pairwise intersections, does not want to preprocess using bounding boxes (maybe most of the regions overlap)
• User wants pairwise intersections, does want to preprocess using bounding boxes
• User already computed bounding boxes (so absolutely should use them to preprocess)

Right now, we technically support a user providing bounding boxes, but setting preprocess_bbox to false. Is this where the confusion stems from? We could explicitly disallow it, or just ignore preprocess_bbox if a user provides bounding boxes. We can also update the documentation to be a bit more clear if that helps.

Few thoughts

1. I still believe it is very confusing to treat empty vector as "Compute them yourself". Empty must mean empty, which means they are not AABBs. I know it is of no use but semantically makes sense.

2. When you have the boxes, then having the flag pre_process=false does not make sense. Of course you want to use the boxes. Good documentation can not make up for confusing API :)

3. The word "maybe" should be only used for optional values, I believe.

Proposed overloads:
Overload 1/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets,
    const std::vector<int>& continuous_revolute_joints,
    bool preprocess_aabb = true,
)

Overload 2/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets,
    const std::vector<int>& continuous_revolute_joints,
    const std::vector<geometry::optimization::Hyperrectangle>& aabbs
)

Overload 3/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets_A,
    const ConvexSets& convex_sets_B,
    const std::vector<int>& continuous_revolute_joints,
    bool preprocess_aabb= true,
)

Overload 4/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets_A,
    const ConvexSets& convex_sets_B,
    const std::vector<int>& continuous_revolute_joints,
    const std::vector<Hyperrectangle>& aabbs_A,
    const std::vector<Hyperrectangle>& aabbs_B,
)

Am I wrong that when we have the AABBs available, we almost always want to use them?

---

geometry/optimization/geodesic_convexity.h line 213 at r1 (raw file):

    const std::vector<int>& continuous_revolute_joints,
    bool preprocess_bbox = true,
    const std::vector<geometry::optimization::Hyperrectangle>& maybe_bboxes =

Ditto

---

geometry/optimization/test/geodesic_convexity_test.cc line 366 at r1 (raw file):

  EXPECT_EQ(CalcPairwiseIntersections(sets_A, sets_B, std::vector<int>{0}, true,
                                      {}, bboxes_B)
                .size(),

nit fix the spacing in these few lines

[cohnt]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/geodesic_convexity.h line 195 at r1 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

Few thoughts

1. I still believe it is very confusing to treat empty vector as "Compute them yourself". Empty must mean empty, which means they are not AABBs. I know it is of no use but semantically makes sense.

2. When you have the boxes, then having the flag pre_process=false does not make sense. Of course you want to use the boxes. Good documentation can not make up for confusing API :)

3. The word "maybe" should be only used for optional values, I believe.

Proposed overloads:
Overload 1/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets,
    const std::vector<int>& continuous_revolute_joints,
    bool preprocess_aabb = true,
)

Overload 2/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets,
    const std::vector<int>& continuous_revolute_joints,
    const std::vector<geometry::optimization::Hyperrectangle>& aabbs
)

Overload 3/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets_A,
    const ConvexSets& convex_sets_B,
    const std::vector<int>& continuous_revolute_joints,
    bool preprocess_aabb= true,
)

Overload 4/4

CalcPairwiseIntersections(
    const ConvexSets& convex_sets_A,
    const ConvexSets& convex_sets_B,
    const std::vector<int>& continuous_revolute_joints,
    const std::vector<Hyperrectangle>& aabbs_A,
    const std::vector<Hyperrectangle>& aabbs_B,
)

Am I wrong that when we have the AABBs available, we almost always want to use them?

1. Using an empty vector as a surrogate for "compute them yourself" also came up in [gcs] Use pointers for optional arguments #21749, and the resolution was to use a pointer to handle a "optional reference" argument. I've switched the function to take a const std::vector<geometry::optimization::Hyperrectangle>* as an argument, where passing a nullptr indicates that the bounding boxes are unknown.
2. I think I've found a way to use these overloads without too much code reuse -- let me know if you're happy with it. That should ensure no one ever wonders what happens if they give bounding boxes, but tell the method not to use them.
3. No longer relevant with the new overload setup.

---

geometry/optimization/geodesic_convexity.h line 213 at r1 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

Ditto

No longer relevant do to the new overloads, as discussed above.

---

geometry/optimization/geodesic_convexity.cc line 441 at r1 (raw file):

        continue;
      }
      // TODO(cohnt) we should still be able to confirm some sets don't overlap

The new changes effect this functionality.

---

geometry/optimization/test/geodesic_convexity_test.cc line 366 at r1 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit fix the spacing in these few lines

This is what the linter outputs, and changing it causes the lint check to fail.

[sadraddini]

Much better than before. Small documentation comment.

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers

---

geometry/optimization/geodesic_convexity.h line 167 at r2 (raw file):

@param bboxes_A is a vector of Hyperrectangles, allowing the user to manually
pass in the AABBs of each set in `convex_sets_A` to avoid recomputation. The

nit add the last sentence as a @warning?

---

geometry/optimization/geodesic_convexity.h line 211 at r2 (raw file):

@param bboxes is a vector of Hyperrectangles, allowing the user to manually pass
in the AABBs of each set in `convex_sets` to avoid recomputation. The function
does not check that the entries are indeed the AABBs corresponding to the sets

nit ditto for warning

[sadraddini]

+@sammy-tri for platform review, please.

[cohnt]

Reviewable status: LGTM missing from assignee sammy-tri(platform)

---

geometry/optimization/geodesic_convexity.h line 167 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit add the last sentence as a @warning?

Done.

---

geometry/optimization/geodesic_convexity.h line 211 at r2 (raw file):

Previously, sadraddini (Sadra Sadraddini) wrote…

nit ditto for warning

Done.

[sammy-tri]

Reviewed 4 of 5 files at r2, 1 of 1 files at r3, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees sammy-tri(platform),sadraddini