Remove scipy.spatial.ConvexHull dependency

[alesgenova]

Related to #14691

---

This change is 

[jwnimmer-tri]

@alesgenova is this ready for feature review yet? If yes, do you have anyone from the Kitware side on deck for review, or would you like someone from TRI to start the review?

[alesgenova]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee svenevs, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @svenevs)

a discussion (no related file):
+@svenevs for feature review please.

---

[svenevs]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee svenevs, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @alesgenova and @svenevs)

---

bindings/pydrake/systems/planar_scenegraph_visualizer.py, line 367 at r1 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

NOTE: It appears that this block of code is never hit through the tests.

bazel run //examples/manipulation_station:joint_teleop -- --setup planar --test

This code unfortunately does not work as we need it to. Before removing scipy the demo looks like this:

With this code:

The comment above about CCW is what we are missing, this is the "winding order" where counterclockwise is the industry standard for presenting vertices. VPolytope does not produce this.

I spent a bit of time looking into the scipy code, adding what I found in one spot but these apply to patch_G above as well.

• See mostly compatible doxygen documentation for runQhull here, and spatial.ConvexHull constructor here.
  • I tried fiddling with rboxCommand ("i") and qhullCommand ("Qt") to no success, but those may be relevant. It may be required that VPolytope::GetMinimalRepresentation receives some optional parameters (const std::string& rboxCommand = "", const std::string& qhullCommand = "") depending on how things work out.
    • Relevant links: rbox manual and qhull manual
    • Choosing "i" and "Qt" respectively was successful in the code above with patch_G, but it fails with patch_V -- I do not know if either of these will be needed in the end, just was referring to what scipy is doing.
• The core issue seems to be that what we are doing and what scipy does result in different orderings. At least as far as I can tell, spatial.ConvexHull docs stating that which for 2-D are guaranteed to be in counterclockwise order is a byproduct of qHull doing this (search counter-clockwise), but Fx is not being used so I am not sure.

  • Regardless, our data is not 2D. However, in the original code inspecting hull.vertices it returns a list of indices into the original patch_G that is also sorted. We may be successful if we can find a way to get that same list of indices out, but I was not successful.

    --- a/geometry/optimization/vpolytope.cc
    +++ b/geometry/optimization/vpolytope.cc
    @@ -144,17 +144,23 @@ VPolytope VPolytope::GetMinimalRepresentation() const {
       }
    
       Eigen::MatrixXd minimal_vertices(vertices_.rows(), qhull.vertexCount());
    +  Eigen::MatrixXd indices(1, qhull.vertexCount());
       size_t j = 0;
       for (const auto& qhull_vertex : qhull.vertexList()) {
         size_t i = 0;
    +    std::cout << "(j=" << j << ", id=" << qhull_vertex.id() << ")\n";
         for (const auto& val : qhull_vertex.point()) {
           minimal_vertices(i, j) = val;
    +      // vvv segfault
    +      // minimal_vertices(i, qhull_vertex.id() - 1) = val;
    +      indices(0, j) = qhull_vertex.id();
           ++i;
         }
         ++j;
       }
    
    -  return VPolytope(minimal_vertices);
    +  // return VPolytope(minimal_vertices);
    +  return VPolytope(indices);
     }
    
     double VPolytope::CalcVolume() const {

  • What remains to be understood is where exactly the mysterious _Qhull class from scipy actually comes from, and to determine how they are populating the list of hull.vertices -- in the patch_G code I do not get the same results (using indices = vpoly.GetMinimalRepresentation().vertices() in the patch_G code, np.sort(indices.astype("int32"), and compare that to hull.vertices from the original code (tip: you can import pdb; pdb.set_trace() when using bazel run just not bazel test).

    • Obviously, returing VPolytope(indices) is only a temporary measure to debug, the final list of indices does not seem the same as scipy is the point, meaning how we interface with qhull seems to differ. But if we find a solution along this route, creating a new helper method could be done.

  • The other option worth looking into is to see if using the existing GetMinimalRepresentation().vertices(), if you can iterate patch_G and accumulate a new list of vertices only if they are found in the minimal representation. In theory since hull.vertices is a list of indices and appears to be in sorted order, this should work, however I was also not successful at reproducing the same ordering as scipy.

Hopefully something in the above is enough to go on, the main point is in either patch_G or patch_V or both cases, the winding order (AKA the order in which the list of vertices are presented) is not acceptable, and that is why the rendering looks so goofy.

[alesgenova]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee svenevs, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @svenevs)

---

bindings/pydrake/systems/planar_scenegraph_visualizer.py, line 367 at r1 (raw file):

Previously, svenevs (Stephen McDowell) wrote…

bazel run //examples/manipulation_station:joint_teleop -- --setup planar --test

This code unfortunately does not work as we need it to. Before removing scipy the demo looks like this:

With this code:

The comment above about CCW is what we are missing, this is the "winding order" where counterclockwise is the industry standard for presenting vertices. VPolytope does not produce this.

I spent a bit of time looking into the scipy code, adding what I found in one spot but these apply to patch_G above as well.

• See mostly compatible doxygen documentation for runQhull here, and spatial.ConvexHull constructor here.
  • I tried fiddling with rboxCommand ("i") and qhullCommand ("Qt") to no success, but those may be relevant. It may be required that VPolytope::GetMinimalRepresentation receives some optional parameters (const std::string& rboxCommand = "", const std::string& qhullCommand = "") depending on how things work out.
    • Relevant links: rbox manual and qhull manual
    • Choosing "i" and "Qt" respectively was successful in the code above with patch_G, but it fails with patch_V -- I do not know if either of these will be needed in the end, just was referring to what scipy is doing.
• The core issue seems to be that what we are doing and what scipy does result in different orderings. At least as far as I can tell, spatial.ConvexHull docs stating that which for 2-D are guaranteed to be in counterclockwise order is a byproduct of qHull doing this (search counter-clockwise), but Fx is not being used so I am not sure.

  • Regardless, our data is not 2D. However, in the original code inspecting hull.vertices it returns a list of indices into the original patch_G that is also sorted. We may be successful if we can find a way to get that same list of indices out, but I was not successful.

    --- a/geometry/optimization/vpolytope.cc
    +++ b/geometry/optimization/vpolytope.cc
    @@ -144,17 +144,23 @@ VPolytope VPolytope::GetMinimalRepresentation() const {
       }
    
       Eigen::MatrixXd minimal_vertices(vertices_.rows(), qhull.vertexCount());
    +  Eigen::MatrixXd indices(1, qhull.vertexCount());
       size_t j = 0;
       for (const auto& qhull_vertex : qhull.vertexList()) {
         size_t i = 0;
    +    std::cout << "(j=" << j << ", id=" << qhull_vertex.id() << ")\n";
         for (const auto& val : qhull_vertex.point()) {
           minimal_vertices(i, j) = val;
    +      // vvv segfault
    +      // minimal_vertices(i, qhull_vertex.id() - 1) = val;
    +      indices(0, j) = qhull_vertex.id();
           ++i;
         }
         ++j;
       }
    
    -  return VPolytope(minimal_vertices);
    +  // return VPolytope(minimal_vertices);
    +  return VPolytope(indices);
     }
    
     double VPolytope::CalcVolume() const {

  • What remains to be understood is where exactly the mysterious _Qhull class from scipy actually comes from, and to determine how they are populating the list of hull.vertices -- in the patch_G code I do not get the same results (using indices = vpoly.GetMinimalRepresentation().vertices() in the patch_G code, np.sort(indices.astype("int32"), and compare that to hull.vertices from the original code (tip: you can import pdb; pdb.set_trace() when using bazel run just not bazel test).

    • Obviously, returing VPolytope(indices) is only a temporary measure to debug, the final list of indices does not seem the same as scipy is the point, meaning how we interface with qhull seems to differ. But if we find a solution along this route, creating a new helper method could be done.

  • The other option worth looking into is to see if using the existing GetMinimalRepresentation().vertices(), if you can iterate patch_G and accumulate a new list of vertices only if they are found in the minimal representation. In theory since hull.vertices is a list of indices and appears to be in sorted order, this should work, however I was also not successful at reproducing the same ordering as scipy.

Hopefully something in the above is enough to go on, the main point is in either patch_G or patch_V or both cases, the winding order (AKA the order in which the list of vertices are presented) is not acceptable, and that is why the rendering looks so goofy.

Thanks for looking into this!
I did some more digging and it looks like the C++ interface to qhull doesn't order the vertices for you. Starting from version 2020.1 they partially implemented it, but it's still a little clunky to use. For this reason I have implemented a manual extra step to order the vertices in the 2D case. I didn't observe a hit in the overall performance (the limiting step is the O(nlogn) sorting), and the visualization looks good again.

[svenevs]

Closing and reopening to re-trigger CI status now that unrelated CI problems are resolved.

[svenevs]

Looking much better, a few more fixes and we're good to go!

Reviewable status: 5 unresolved discussions, LGTM missing from assignee svenevs, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @alesgenova and @svenevs)

---

geometry/optimization/vpolytope.cc, line 138 at r2 (raw file):

}

Eigen::MatrixXd OrderCounterClockwise(const Eigen::MatrixXd& vertices) {

For functions that are local to the translation unit, drake puts them at the top in an anonymous namespace. So this function should move to just above VPolytope::VPolytope(const Eigen::Ref<const Eigen::MatrixXd>& vertices)

namespace {

Eigen::MatrixXd OrderCounterClockwise(const Eigen::MatrixXd& vertices) {
  // ...
}

}  // namespace

It would be good to put a brief comment above the function, something like /* For 2D data, return a copy of vertices` in counter-clockwise order. */

---

geometry/optimization/vpolytope.cc, line 142 at r2 (raw file):

  size_t n = vertices.cols();

  DRAKE_ASSERT(dim == 2);

I believe this needs to change to DRAKE_DEMAND(dim == 2);, my read of drake_assert.h is that asserts are not always armed but dim == 2 is a hard requirement for the logic below.

---

geometry/optimization/vpolytope.cc, line 199 at r2 (raw file):

  }

  if (vertices_.rows() == 2) {

BTW since there are changes that need to be made, I think it would be good to add a brief comment explaining that the CPP QHull wrapper has some issues with returning CCW ordering.

---

bindings/pydrake/systems/planar_scenegraph_visualizer.py, line 367 at r1 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

Thanks for looking into this!
I did some more digging and it looks like the C++ interface to qhull doesn't order the vertices for you. Starting from version 2020.1 they partially implemented it, but it's still a little clunky to use. For this reason I have implemented a manual extra step to order the vertices in the 2D case. I didn't observe a hit in the overall performance (the limiting step is the O(nlogn) sorting), and the visualization looks good again.

OK, awesome -- this is great! The qhull docs are vague at best, but as far as I can tell we don't need to worry about the performance here since the demo appears to run smoothly and realistically scipy is probably doing the same thing behind the scenes -- somebody is sorting it somewhere.

---

bindings/pydrake/test/geometry_optimization_test.py, line 189 at r2 (raw file):

        # Ensure counter clockwise order
        self.assertAlmostEqual(self._calculate_perimeter(vpoly.vertices()),
                               2 * np.pi * r, delta=1e-3)

Why does comparing against the calculated perimeter actually check counter clockwise ordering? It seems to me that this is just validating the size of the hull, not the ordering. If it does, a brief comment explaining how that works (here and in cpp side) would be helpful to future developers.

If it is not actually verifying the ordering, I don't believe we need to test that -- it's a special case of 2D only for the planar renderings which we have already confirmed works. I think creating a test to verify the ordering is not worth the time it would take (?)

[svenevs]

Reviewable status: 6 unresolved discussions, LGTM missing from assignee svenevs, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @alesgenova and @svenevs)

---

geometry/optimization/test/vpolytope_test.cc, line 481 at r2 (raw file):

double CalcPerimeter(const Eigen::MatrixXd& vertices) {
  DRAKE_ASSERT(vertices.rows() == 2);

This should change to DRAKE_DEMAND as well.

[alesgenova]

---

geometry/optimization/vpolytope.cc, line 199 at r2 (raw file):

Previously, svenevs (Stephen McDowell) wrote…

BTW since there are changes that need to be made, I think it would be good to add a brief comment explaining that the CPP QHull wrapper has some issues with returning CCW ordering.

done

[alesgenova]

Reviewable status: 5 unresolved discussions, LGTM missing from assignee svenevs, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @svenevs)

---

geometry/optimization/vpolytope.cc, line 138 at r2 (raw file):

Previously, svenevs (Stephen McDowell) wrote…

For functions that are local to the translation unit, drake puts them at the top in an anonymous namespace. So this function should move to just above VPolytope::VPolytope(const Eigen::Ref<const Eigen::MatrixXd>& vertices)

namespace {

Eigen::MatrixXd OrderCounterClockwise(const Eigen::MatrixXd& vertices) {
  // ...
}

}  // namespace

It would be good to put a brief comment above the function, something like /* For 2D data, return a copy of vertices` in counter-clockwise order. */

Done.

---

geometry/optimization/vpolytope.cc, line 142 at r2 (raw file):

Previously, svenevs (Stephen McDowell) wrote…

I believe this needs to change to DRAKE_DEMAND(dim == 2);, my read of drake_assert.h is that asserts are not always armed but dim == 2 is a hard requirement for the logic below.

Done.

---

geometry/optimization/test/vpolytope_test.cc, line 481 at r2 (raw file):

Previously, svenevs (Stephen McDowell) wrote…

This should change to DRAKE_DEMAND as well.

Done.

---

bindings/pydrake/test/geometry_optimization_test.py, line 189 at r2 (raw file):

Previously, svenevs (Stephen McDowell) wrote…

Why does comparing against the calculated perimeter actually check counter clockwise ordering? It seems to me that this is just validating the size of the hull, not the ordering. If it does, a brief comment explaining how that works (here and in cpp side) would be helpful to future developers.

If it is not actually verifying the ordering, I don't believe we need to test that -- it's a special case of 2D only for the planar renderings which we have already confirmed works. I think creating a test to verify the ordering is not worth the time it would take (?)

Added a comment as to why we are calculating the perimeter (renamed it to path length).

[svenevs]

The only outstanding question is whether this should be release notes: none or release notes: yes. I think the argument in favor if yes is that if something did go wrong, release notes describing that PlanarSceneGraphVisualizer now use a different method for computing the convex hull would help users find it. (In the squashed commit message I would add PlanarSceneGraphVisualizer now uses VPolytope::GetMinimalRepresentation to compute the convex hull.)

+@ggould-tri for platform review please. Useful context:

1. bazel run //examples/manipulation_station:joint_teleop -- --setup planar --test is running as expected, compared to previous issues related to ordering of vertices.
2. There are now tests in place in both python and C++ to confirm that the "path length" of the vertices returned by VPolytope::GetMinimalRepresentation in the 2D case are approximately the same as the length of the perimeter of a circle. If the ordering is random (as returned by C++ qhull bindings), it will be longer (many lines darting across a circle). Officially the test does not validate CCW ordering (as noted in the comments, it would pass if it were clockwise), but this test combined with (1) appearing visually correct confirm the ordering is valid. I do not believe introducing a test to check specifically for CCW ordering would be productive.

Reviewed 1 of 1 files at r1, 3 of 3 files at r3, all commit messages.
Dismissed @alesgenova from a discussion.
Reviewable status: LGTM missing from assignee ggould-tri(platform), missing label for release notes (waiting on @ggould-tri)

---

geometry/optimization/vpolytope.cc, line 138 at r2 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

Done.

OK great, thanks!

---

geometry/optimization/vpolytope.cc, line 142 at r2 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

Done.

OK looks good to me.

---

geometry/optimization/vpolytope.cc, line 199 at r2 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

done

OK great, thanks!

---

geometry/optimization/test/vpolytope_test.cc, line 481 at r2 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

Done.

OK great, thanks!

---

bindings/pydrake/test/geometry_optimization_test.py, line 189 at r2 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

Added a comment as to why we are calculating the perimeter (renamed it to path length).

OK looks good to me now, changing to "path length" and the comments explaining how this test works (here and in C++ test) makes much more sense now. Nice work!

[ggould-tri]

@svenevs My inclination is that if there's any case for release notes: yes then we should do it. I've noted your proposed commit message below to make sure it gets in and saves Rico some hunting.
+(release notes: yes)

@alesgenova with a handful of minor issues.

Reviewed 1 of 1 files at r1, 3 of 3 files at r3, all commit messages.
Reviewable status: 10 unresolved discussions (waiting on @alesgenova)

---

-- commits, line 2 at r3:
minor: This commit message needs enough information for a release note. Per @svenevs , I suggest:

PlanarSceneGraphVisualizer now uses VPolytope::GetMinimalRepresentation to compute the convex hull.

---

geometry/optimization/vpolytope.cc, line 5 at r3 (raw file):

#include <algorithm>
#include <limits>
#include <memory>

nit: If <algorithm> was just for std::iota, <numeric> is the more parsimonious include.
nit: Needs <cmath> for std::atan2.

Suggestion:

#include <cmath>
#include <limits>
#include <memory>
#include <numeric>

---

geometry/optimization/vpolytope.cc, line 34 at r3 (raw file):

Eigen::MatrixXd OrderCounterClockwise(const Eigen::MatrixXd& vertices) {
  /* Given a matrix containing a set of 2D vertices, return a copy
     of the matrix where the vertices are ordered counter-clockwise. */

minor: Conventionally interface/contract comments go before the definition (i.e. two lines earlier) rather than inside it.

---

geometry/optimization/vpolytope.cc, line 34 at r3 (raw file):

Eigen::MatrixXd OrderCounterClockwise(const Eigen::MatrixXd& vertices) {
  /* Given a matrix containing a set of 2D vertices, return a copy
     of the matrix where the vertices are ordered counter-clockwise. */

minor: "counter-clockwise from the positive X axis" would be more specific.

---

geometry/optimization/vpolytope.cc, line 36 at r3 (raw file):

     of the matrix where the vertices are ordered counter-clockwise. */
  size_t dim = vertices.rows();
  size_t n = vertices.cols();

minor: num_vertices would be a clearer name. Alternatively mention n in the comment above as the number of 2d vertices.

---

geometry/optimization/vpolytope.cc, line 48 at r3 (raw file):

  std::iota(indices.begin(), indices.end(), 0);

  for (auto& i : indices) {

nit: const

Suggestion:

const auto& i

---

geometry/optimization/vpolytope.cc, line 56 at r3 (raw file):

  center_y /= n;

  for (auto& i : indices) {

nit: const

Suggestion:

const auto& i

---

geometry/optimization/vpolytope.cc, line 57 at r3 (raw file):

  for (auto& i : indices) {
    auto x = vertices.col(i)[0] - center_x;

nit: This and the line below are borderline cases for auto; at author's discretion consider just calling them const double.

Ref: https://drake.mit.edu/styleguide/cppguide.html#Type_deduction

---

geometry/optimization/vpolytope.cc, line 64 at r3 (raw file):

  std::sort(indices.begin(), indices.end(), [&angles](size_t a, size_t b){
    return angles[a] > angles[b];
  });

minor: The indentation here is misleading, since the } is not of the same scope as the sort. I am a bit surprised the linter didn't object.
A suggested indentation below, although others are equally valid.

Suggestion:

  std::sort(indices.begin(), indices.end(), [&angles](size_t a, size_t b){
      return angles[a] > angles[b];
    });

---

geometry/optimization/vpolytope.cc, line 207 at r3 (raw file):

  // The qhull C++ interface iterates over the vertices in no specific order.
  // For the 2D case, reorder the vertices according to the counter-clockwise
  // convention.

minor: What is the implication for dimensions other than 2 -- is the output order unspecified in those cases? If so that should be mentioned in the header.

---

geometry/optimization/test/vpolytope_test.cc, line 514 at r3 (raw file):

    // If the vertices are in clockwise/counter-clockwise order,
    // the length of the path will coincide with the perimeter of a
    // square.

BTW: Elegant test!

[ggould-tri]

---

geometry/optimization/vpolytope.cc, line 5 at r3 (raw file):

Previously, ggould-tri wrote…

nit: If <algorithm> was just for std::iota, <numeric> is the more parsimonious include.
nit: Needs <cmath> for std::atan2.

Retracting (mostly) as I see better what's going on here.

[ggould-tri]

Reviewable status: 10 unresolved discussions (waiting on @alesgenova)

---

geometry/optimization/vpolytope.cc, line 5 at r3 (raw file):

#include <algorithm>
#include <limits>
#include <memory>

nit: Consider including <numeric> for iota and <cmath> for atan2. I expect you're picking them up transitively through your geometry include, but it's better to be specific.

[alesgenova]

---

geometry/optimization/vpolytope.cc, line 34 at r3 (raw file):

Previously, ggould-tri wrote…

minor: "counter-clockwise from the positive X axis" would be more specific.

Technically it's from the negative X axis, since the angle returned by atan2 is in the [-pi, +pi) range. I'll add "from the negative X axis" to the function comment. I we want it to actually be ordered starting from the positive X axis I would need to add the following snippet:

double angle = std::atan2(y, x);
if (angle < 0) {
  angle += 2 * M_PI;
}
angles[i] = angle;

Let me know if strict positive axis is something we want to enforce.

[ggould-tri]

Reviewable status: 10 unresolved discussions (waiting on @alesgenova)

---

geometry/optimization/vpolytope.cc, line 34 at r3 (raw file):

Previously, alesgenova (Alessandro Genova) wrote…

Technically it's from the negative X axis, since the angle returned by atan2 is in the [-pi, +pi) range. I'll add "from the negative X axis" to the function comment. I we want it to actually be ordered starting from the positive X axis I would need to add the following snippet:

double angle = std::atan2(y, x);
if (angle < 0) {
  angle += 2 * M_PI;
}
angles[i] = angle;

Let me know if strict positive axis is something we want to enforce.

Heh, the fact that I got it wrong proves the clarifying comment is needed!
I don't much care which axis so long as we say it :-)

[svenevs]

Reviewed 2 of 2 files at r4, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),svenevs (waiting on @alesgenova)

---

geometry/optimization/test/vpolytope_test.cc, line 514 at r3 (raw file):

Previously, ggould-tri wrote…

BTW: Elegant test!

^^ Agreed 🙂