-
Notifications
You must be signed in to change notification settings - Fork 1.2k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Remove scipy.spatial.ConvexHull dependency #16765
Remove scipy.spatial.ConvexHull dependency #16765
Conversation
@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? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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, andspatial.ConvexHull
constructor here.- I tried fiddling with
rboxCommand
("i"
) andqhullCommand
("Qt"
) to no success, but those may be relevant. It may be required thatVPolytope::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 withpatch_G
, but it fails withpatch_V
-- I do not know if either of these will be needed in the end, just was referring to what scipy is doing.
- I tried fiddling with
- 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 thatwhich for 2-D are guaranteed to be in counterclockwise order
is a byproduct ofqHull
doing this (searchcounter-clockwise
), butFx
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 originalpatch_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 ofhull.vertices
-- in thepatch_G
code I do not get the same results (usingindices = vpoly.GetMinimalRepresentation().vertices()
in thepatch_G
code,np.sort(indices.astype("int32")
, and compare that tohull.vertices
from the original code (tip: you canimport pdb; pdb.set_trace()
when usingbazel run
just notbazel 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.
- Obviously, returing
-
The other option worth looking into is to see if using the existing
GetMinimalRepresentation().vertices()
, if you can iteratepatch_G
and accumulate a new list of vertices only if they are found in the minimal representation. In theory sincehull.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.
fb35a95
to
064f2de
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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, andspatial.ConvexHull
constructor here.
- I tried fiddling with
rboxCommand
("i"
) andqhullCommand
("Qt"
) to no success, but those may be relevant. It may be required thatVPolytope::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 withpatch_G
, but it fails withpatch_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 thatwhich for 2-D are guaranteed to be in counterclockwise order
is a byproduct ofqHull
doing this (searchcounter-clockwise
), butFx
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 originalpatch_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 ofhull.vertices
-- in thepatch_G
code I do not get the same results (usingindices = vpoly.GetMinimalRepresentation().vertices()
in thepatch_G
code,np.sort(indices.astype("int32")
, and compare that tohull.vertices
from the original code (tip: you canimport pdb; pdb.set_trace()
when usingbazel run
just notbazel 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 iteratepatch_G
and accumulate a new list of vertices only if they are found in the minimal representation. In theory sincehull.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
orpatch_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.
064f2de
to
cea565d
Compare
Closing and reopening to re-trigger CI status now that unrelated CI problems are resolved. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 (?)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
cea565d
to
ada919e
Compare
geometry/optimization/vpolytope.cc, line 199 at r2 (raw file): Previously, svenevs (Stephen McDowell) wrote…
done |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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) { // ... } } // namespaceIt 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 ofdrake_assert.h
is that asserts are not always armed butdim == 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).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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:
bazel run //examples/manipulation_station:joint_teleop -- --setup planar --test
is running as expected, compared to previous issues related to ordering of vertices.- 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!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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!
geometry/optimization/vpolytope.cc, line 5 at r3 (raw file): Previously, ggould-tri wrote…
Retracting (mostly) as I see better what's going on here. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
geometry/optimization/vpolytope.cc, line 34 at r3 (raw file): Previously, ggould-tri 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:
Let me know if strict positive axis is something we want to enforce. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 :-)
…on to compute the convex hull
ada919e
to
c0f1c61
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 🙂
…on to compute the convex hull (RobotLocomotion#16765)
…on to compute the convex hull (RobotLocomotion#16765)
…on to compute the convex hull (RobotLocomotion#16765)
Related to #14691
This change is![Reviewable](https://camo.githubusercontent.com/1541c4039185914e83657d3683ec25920c672c6c5c7ab4240ee7bff601adec0b/68747470733a2f2f72657669657761626c652e696f2f7265766965775f627574746f6e2e737667)