From 5fb945d4f2ae126192b1809895a32e8253de2136 Mon Sep 17 00:00:00 2001
From: Russ Tedrake <russt@mit.edu>
Date: Mon, 17 Apr 2023 17:07:07 -0400
Subject: [PATCH] Add geometry::optimization::MakeConvexSets (#19207)

This is a simple method to help initialize lists of ConvexSets.

Co-Authored-By: Jeremy Nimmer <jeremy.nimmer@tri.global>
---
 geometry/optimization/BUILD.bazel             |  1 +
 geometry/optimization/convex_set.h            | 16 ++++++
 geometry/optimization/test/convex_set_test.cc | 53 +++++++++++++++++++
 3 files changed, 70 insertions(+)

diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel
index 83231689dc11..7ec719e45231 100644
--- a/geometry/optimization/BUILD.bazel
+++ b/geometry/optimization/BUILD.bazel
@@ -160,6 +160,7 @@ drake_cc_googletest(
     deps = [
         ":convex_set",
         "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:limit_malloc",
         "//solvers:clp_solver",
     ],
 )
diff --git a/geometry/optimization/convex_set.h b/geometry/optimization/convex_set.h
index 4e8754055089..475fd501aa09 100644
--- a/geometry/optimization/convex_set.h
+++ b/geometry/optimization/convex_set.h
@@ -244,6 +244,22 @@ std::unique_ptr<ConvexSet> ConvexSetCloner(const ConvexSet& other) {
 instances. */
 typedef std::vector<copyable_unique_ptr<ConvexSet>> ConvexSets;
 
+/** Helper function that allows the ConvexSets to be initialized from arguments
+containing ConvexSet references, or unique_ptr<ConvexSet> instances, or any
+object that can be assigned to ConvexSets::value_type. */
+template <typename... Args>
+ConvexSets MakeConvexSets(Args&&... args) {
+  ConvexSets sets;
+  constexpr size_t N = sizeof...(args);
+  sets.resize(N);
+  // This is a "constexpr for" loop for 0 <= I < N.
+  auto args_tuple = std::forward_as_tuple(std::forward<Args>(args)...);
+  [&]<size_t... I>(std::integer_sequence<size_t, I...> &&) {
+    ((sets[I] = std::get<I>(std::move(args_tuple))), ...);
+  }(std::make_index_sequence<N>{});
+  return sets;
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake
diff --git a/geometry/optimization/test/convex_set_test.cc b/geometry/optimization/test/convex_set_test.cc
index d0384fdba11d..af46fcf457d5 100644
--- a/geometry/optimization/test/convex_set_test.cc
+++ b/geometry/optimization/test/convex_set_test.cc
@@ -4,6 +4,7 @@
 
 #include "drake/common/is_approx_equal_abstol.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/common/test_utilities/limit_malloc.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
 #include "drake/geometry/optimization/point.h"
 
@@ -14,6 +15,7 @@ namespace {
 
 using Eigen::Vector2d;
 using Eigen::Vector3d;
+using test::LimitMalloc;
 
 GTEST_TEST(ConvexSetsTest, BasicTest) {
   ConvexSets sets;
@@ -81,6 +83,57 @@ arrangement of boxes:
   EXPECT_FALSE(set_A.IntersectsWith(set_C));
   EXPECT_FALSE(set_C.IntersectsWith(set_A));
 }
+
+GTEST_TEST(MakeConvexSetsTest, Basic) {
+  HPolyhedron box = HPolyhedron::MakeUnitBox(2);
+  ConvexSets sets =
+      MakeConvexSets(box, box.Clone(), Point(Vector3d(1.0, 2.0, 3.0)));
+
+  EXPECT_EQ(sets.size(), 3);
+  EXPECT_EQ(sets[0]->ambient_dimension(), 2);
+  EXPECT_EQ(sets[1]->ambient_dimension(), 2);
+  EXPECT_EQ(sets[2]->ambient_dimension(), 3);
+}
+
+// A mutable lvalue reference is copied, not moved.
+GTEST_TEST(MakeConvexSetsTest, MutableLvalueReference) {
+  const HPolyhedron box = HPolyhedron::MakeUnitBox(2);
+  std::unique_ptr<ConvexSet> box_clone = box.Clone();
+  ConvexSets sets = MakeConvexSets(box_clone);
+  EXPECT_EQ(sets.size(), 1);
+  EXPECT_NE(box_clone.get(), nullptr);
+}
+
+// The amount of copying is as small as possible.
+GTEST_TEST(MakeConvexSetsTest, NoExtraCopying) {
+  const HPolyhedron box = HPolyhedron::MakeUnitBox(2);
+
+  // A `unique_ptr<ConvexSet>` is moved into place, no copies.
+  // The only allocation is the std::vector storage itself.
+  {
+    std::unique_ptr<ConvexSet> box1{box.Clone()};
+    std::unique_ptr<ConvexSet> box2{box.Clone()};
+    LimitMalloc guard({.max_num_allocations = 1});
+    MakeConvexSets(std::move(box1), std::move(box2));
+  }
+
+  // A `copyable_unique_ptr<ConvexSet>` is moved into place, no copies.
+  {
+    copyable_unique_ptr<ConvexSet> box1{box.Clone()};
+    copyable_unique_ptr<ConvexSet> box2{box.Clone()};
+    LimitMalloc guard({.max_num_allocations = 1});
+    MakeConvexSets(std::move(box1), std::move(box2));
+  }
+
+  // A `const ConvexSet&` is copied just once.
+  {
+    const int box_clone_num_allocs = 3;  // HPolyhedron, A_ , b_.
+    const int num = 1 + box_clone_num_allocs;
+    LimitMalloc guard({.max_num_allocations = num, .min_num_allocations = num});
+    MakeConvexSets(box);
+  }
+};
+
 }  // namespace
 
 }  // namespace optimization