VPolytope implements convex geometry. (RobotLocomotion#16351)

* VPolytope implements convex geometry.

geometry/BUILD.bazel

@@ -42,6 +42,7 @@ drake_cc_package_library(
         ":meshcat_visualizer_params",
         ":proximity_engine",
         ":proximity_properties",
+        ":read_obj",
         ":rgba",
         ":scene_graph",
         ":scene_graph_inspector",
@@ -63,6 +64,7 @@ drake_cc_library(
         ":geometry_ids",
         ":geometry_roles",
         ":internal_geometry",
+        ":read_obj",
         ":shape_specification",
         ":utilities",
         "//common",
@@ -72,7 +74,6 @@ drake_cc_library(
         "//math",
         "@fcl",
         "@fmt",
-        "@tinyobjloader",
     ],
 )
 
@@ -295,6 +296,17 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "read_obj",
+    srcs = ["read_obj.cc"],
+    hdrs = ["read_obj.h"],
+    deps = [
+        "//common:essential",
+        "@fmt",
+        "@tinyobjloader",
+    ],
+)
+
 drake_cc_library(
     name = "shape_specification",
     srcs = ["shape_specification.cc"],
@@ -530,6 +542,8 @@ filegroup(
         "test/extruded_u.obj",
         "test/forbidden_two_cubes.obj",
         "test/non_convex_mesh.obj",
+        "test/octahedron.mtl",
+        "test/octahedron.obj",
         "test/quad_cube.mtl",
         "test/quad_cube.obj",
     ],
@@ -713,6 +727,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "read_obj_test",
+    data = [":test_obj_files"],
+    deps = [
+        ":read_obj",
+        "//common:find_resource",
+        "//common/test_utilities",
+    ],
+)
+
 install(
     name = "install",
     visibility = ["//visibility:public"],

geometry/optimization/BUILD.bazel

@@ -161,6 +161,7 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "vpolytope_test",
+    data = ["//geometry:test_obj_files"],
     deps = [
         ":convex_set",
         ":test_utilities",

geometry/optimization/test/vpolytope_test.cc

@@ -5,6 +5,7 @@
 #include <gtest/gtest.h>
 
 #include "drake/common/eigen_types.h"
+#include "drake/common/find_resource.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/geometry/geometry_frame.h"
@@ -145,6 +146,74 @@ GTEST_TEST(VPolytopeTest, ArbitraryBoxTest) {
   EXPECT_FALSE(V_F.PointInSet(X_FW * out_W, kTol));
 }
 
+// Check if the set of vertices equals to the set of vertices_expected.
+void CheckVertices(const Eigen::Ref<const Eigen::Matrix3Xd>& vertices,
+                   const Eigen::Ref<const Eigen::Matrix3Xd>& vertices_expected,
+                   double tol) {
+  EXPECT_EQ(vertices.cols(), vertices_expected.cols());
+  const int num_vertices = vertices.cols();
+  for (int i = 0; i < num_vertices; ++i) {
+    bool found_match = false;
+    for (int j = 0; j < num_vertices; ++j) {
+      if (CompareMatrices(vertices.col(i), vertices_expected.col(j), tol)) {
+        found_match = true;
+        break;
+      }
+    }
+    EXPECT_TRUE(found_match);
+  }
+}
+
+GTEST_TEST(VPolytopeTest, OctahedronTest) {
+  const RigidTransformd X_WG(math::RollPitchYawd(.1, .2, 3),
+                             Vector3d(-4.0, -5.0, -6.0));
+  auto [scene_graph, geom_id] = MakeSceneGraphWithShape(
+      Convex(FindResourceOrThrow("drake/geometry/test/octahedron.obj")), X_WG);
+  auto context = scene_graph->CreateDefaultContext();
+  auto query =
+      scene_graph->get_query_output_port().Eval<QueryObject<double>>(*context);
+  VPolytope V(query, geom_id);
+  EXPECT_EQ(V.vertices().cols(), 6);
+
+  EXPECT_EQ(V.ambient_dimension(), 3);
+
+  Eigen::Matrix<double, 6, 3> p_GV_expected;
+  // clang-format off
+  p_GV_expected << 1, 1, 0,
+                   1, -1, 0,
+                   -1, 1, 0,
+                   -1, -1, 0,
+                   0, 0, std::sqrt(2),
+                   0, 0, -std::sqrt(2);
+  // clang-format on
+  CheckVertices(V.vertices(), X_WG * p_GV_expected.transpose(), 1E-9);
+}
+
+GTEST_TEST(VPolytopeTest, NonconvexMesh) {
+  auto [scene_graph, geom_id] = MakeSceneGraphWithShape(
+      Convex(FindResourceOrThrow("drake/geometry/test/non_convex_mesh.obj")),
+      RigidTransformd{});
+  auto context = scene_graph->CreateDefaultContext();
+  auto query =
+      scene_graph->get_query_output_port().Eval<QueryObject<double>>(*context);
+  VPolytope V(query, geom_id);
+
+  // The non-convex mesh contains 5 vertices, but the convex hull contains only
+  // 4 vertices.
+  const int num_vertices = 4;
+  EXPECT_EQ(V.vertices().cols(), num_vertices);
+  EXPECT_EQ(V.ambient_dimension(), 3);
+  Eigen::Matrix<double, 4, 3> vertices_expected;
+  // clang-format off
+  vertices_expected << 0, 0, 0,
+                       1, 0, 0,
+                       0, 1, 0,
+                       0, 0, 1;
+  // clang-format on
+  const double tol = 1E-12;
+  CheckVertices(V.vertices(), vertices_expected.transpose(), tol);
+}
+
 GTEST_TEST(VPolytopeTest, UnitBox6DTest) {
   VPolytope V = VPolytope::MakeUnitBox(6);
   EXPECT_EQ(V.ambient_dimension(), 6);

geometry/optimization/vpolytope.cc

@@ -8,6 +8,7 @@
 #include <libqhullcpp/QhullVertexSet.h>
 
 #include "drake/common/is_approx_equal_abstol.h"
+#include "drake/geometry/read_obj.h"
 #include "drake/solvers/solve.h"
 
 namespace drake {
@@ -224,6 +225,7 @@ void VPolytope::ImplementGeometry(const Box& box, void* data) {
   const double x = box.width() / 2.0;
   const double y = box.depth() / 2.0;
   const double z = box.height() / 2.0;
+  DRAKE_ASSERT(data != nullptr);
   Matrix3Xd* vertices = static_cast<Matrix3Xd*>(data);
   vertices->resize(3, 8);
   // clang-format off
@@ -233,6 +235,36 @@ void VPolytope::ImplementGeometry(const Box& box, void* data) {
   // clang-format on
 }
 
+void VPolytope::ImplementGeometry(const Convex& convex, void* data) {
+  DRAKE_ASSERT(data != nullptr);
+  const auto [tinyobj_vertices, faces, num_faces] = internal::ReadObjFile(
+      convex.filename(), convex.scale(), false /* triangulate */);
+  unused(faces);
+  unused(num_faces);
+  orgQhull::Qhull qhull;
+  const int dim = 3;
+  std::vector<double> tinyobj_vertices_flat(tinyobj_vertices->size() * dim);
+  for (int i = 0; i < static_cast<int>(tinyobj_vertices->size()); ++i) {
+    for (int j = 0; j < dim; ++j) {
+      tinyobj_vertices_flat[dim * i + j] = (*tinyobj_vertices)[i](j);
+    }
+  }
+  qhull.runQhull("", dim, tinyobj_vertices->size(),
+                 tinyobj_vertices_flat.data(), "");
+  if (qhull.qhullStatus() != 0) {
+    throw std::runtime_error(
+        fmt::format("Qhull terminated with status {} and  message:\n{}",
+                    qhull.qhullStatus(), qhull.qhullMessage()));
+  }
+  Matrix3Xd* vertices = static_cast<Matrix3Xd*>(data);
+  vertices->resize(3, qhull.vertexCount());
+  int vertex_count = 0;
+  for (const auto& qhull_vertex : qhull.vertexList()) {
+    vertices->col(vertex_count++) =
+        Eigen::Map<Eigen::Vector3d>(qhull_vertex.point().toStdVector().data());
+  }
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/vpolytope.h

@@ -88,9 +88,7 @@ class VPolytope final : public ConvexSet {
   // Implement support shapes for the ShapeReifier interface.
   using ShapeReifier::ImplementGeometry;
   void ImplementGeometry(const Box& box, void* data) final;
-  // TODO(russt): Support ImplementGeometry(const Convex& convex, ...), but
-  // currently it would require e.g. digging ReadObjForConvex out of
-  // proximity_engine.cc.
+  void ImplementGeometry(const Convex& convex, void* data) final;
 
   Eigen::MatrixXd vertices_;
 };