Fix ConvexSet move operations

geometry/optimization/convex_set.h

@@ -7,6 +7,7 @@
 
 #include "drake/common/copyable_unique_ptr.h"
 #include "drake/common/drake_assert.h"
+#include "drake/common/reset_after_move.h"
 #include "drake/geometry/geometry_ids.h"
 #include "drake/geometry/query_object.h"
 #include "drake/geometry/shape_specification.h"
@@ -180,7 +181,9 @@ class ConvexSet : public ShapeReifier {
   /** Implements non-virtual base class serialization. */
   template <typename Archive>
   void Serialize(Archive* a) {
-    a->Visit(MakeNameValue("ambient_dimension", &ambient_dimension_));
+    // Visit the mutable reference inside the reset_after_move wrapper.
+    int& ambient_dimension = ambient_dimension_;
+    a->Visit(DRAKE_NVP(ambient_dimension));
   }
 
   /** Non-virtual interface implementation for Clone(). */
@@ -238,7 +241,21 @@ class ConvexSet : public ShapeReifier {
   DoToShapeWithPose() const = 0;
 
  private:
-  int ambient_dimension_{0};
+  // The reset_after_move wrapper adjusts ConvexSet's default move constructor
+  // and move assignment operator to set the ambient dimension of a moved-from
+  // object back to zero. This is essential to keep the ambient dimension in
+  // sync with any moved-from member fields in concrete ConvexSet subclasses.
+  //
+  // For example, the `Eigen::VectorXd x_` member field of `Point` will be moved
+  // out, ending up with `x_.size() == 0` on the moved-from Point. To maintain
+  // the invariant that `x_.size() == ambient_dimension_`, we need to zero the
+  // dimension when `x_` is moved-from.
+  //
+  // Similarly, for subclasses that are composite sets (e.g., CartesianProduct)
+  // the `ConvexSets sets_` vector becomes empty when moved-from. To maintain
+  // an invariant like `∑(set.size() for set in sets_) == ambient_dimension_`,
+  // we need to zero the dimension when `sets_` is moved-from.
+  reset_after_move<int> ambient_dimension_;
 };
 
 /** Provides the recommended container for passing a collection of ConvexSet

geometry/optimization/test/cartesian_product_test.cc

@@ -66,6 +66,23 @@ GTEST_TEST(CartesianProductTest, DefaultCtor) {
   EXPECT_FALSE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
 }
 
+GTEST_TEST(CartesianProductTest, Move) {
+  const Point P1(Vector2d{1.2, 3.4}), P2(Vector2d{5.6, 7.8});
+  CartesianProduct orig(P1, P2);
+
+  // A move-constructed CartesianProduct takes over the original data.
+  CartesianProduct dut(std::move(orig));
+  EXPECT_EQ(dut.num_factors(), 2);
+  EXPECT_EQ(dut.ambient_dimension(), 4);
+
+  // The old set is in a valid but unspecified state. For convenience we'll
+  // assert that it's empty, but that's not the only valid implementation,
+  // just the one we happen to currently use.
+  EXPECT_EQ(orig.num_factors(), 0);
+  EXPECT_EQ(orig.ambient_dimension(), 0);
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(CartesianProductTest, FromSceneGraph) {
   const RigidTransformd X_WG{math::RollPitchYawd(.1, .2, 3),
                              Vector3d{.5, .87, .1}};

geometry/optimization/test/hpolyhedron_test.cc

@@ -89,6 +89,24 @@ GTEST_TEST(HPolyhedronTest, UnitBoxTest) {
   EXPECT_TRUE(CompareMatrices(b, H_scene_graph.b()));
 }
 
+GTEST_TEST(HPolyhedronTest, Move) {
+  Matrix<double, 6, 3> A;
+  A << Matrix3d::Identity(), -Matrix3d::Identity();
+  Vector6d b = Vector6d::Ones();
+  HPolyhedron orig(A, b);
+
+  // A move-constructed HPolyhedron takes over the original data.
+  HPolyhedron dut(std::move(orig));
+  EXPECT_EQ(dut.ambient_dimension(), 3);
+  EXPECT_TRUE(CompareMatrices(dut.A(), A));
+  EXPECT_TRUE(CompareMatrices(dut.b(), b));
+
+  // The old HPolyhedron is in a valid but unspecified state.
+  EXPECT_EQ(orig.A().cols(), orig.ambient_dimension());
+  EXPECT_EQ(orig.b().size(), orig.ambient_dimension());
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(HPolyhedronTest, ConstructorFromVPolytope) {
   Eigen::Matrix<double, 3, 4> vert1;
   // clang-format off

geometry/optimization/test/hyperellipsoid_test.cc

@@ -98,6 +98,23 @@ GTEST_TEST(HyperellipsoidTest, DefaultCtor) {
   EXPECT_FALSE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
 }
 
+GTEST_TEST(HyperellipsoidTest, Move) {
+  const Eigen::Matrix3d A = Eigen::Matrix3d::Identity();
+  const Vector3d center = Vector3d::Zero();
+  Hyperellipsoid orig(A, center);
+
+  // A move-constructed Hyperellipsoid takes over the original data.
+  Hyperellipsoid dut(std::move(orig));
+  EXPECT_EQ(dut.ambient_dimension(), 3);
+  EXPECT_TRUE(CompareMatrices(dut.A(), A));
+  EXPECT_TRUE(CompareMatrices(dut.center(), center));
+
+  // The old Hyperellipsoid is in a valid but unspecified state.
+  EXPECT_EQ(orig.A().cols(), orig.ambient_dimension());
+  EXPECT_EQ(orig.center().size(), orig.ambient_dimension());
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(HyperellipsoidTest, ScaledSphereTest) {
   const double radius = 0.1;
   auto [scene_graph, geom_id] =

geometry/optimization/test/intersection_test.cc

@@ -68,6 +68,24 @@ GTEST_TEST(IntersectionTest, DefaultCtor) {
   EXPECT_FALSE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
 }
 
+GTEST_TEST(IntersectionTest, Move) {
+  const Point P1(Vector2d{0.1, 1.2});
+  HPolyhedron H1 = HPolyhedron::MakeBox(Vector2d{0, 0}, Vector2d{2, 2});
+  Intersection orig(P1, H1);
+
+  // A move-constructed Intersection takes over the original data.
+  Intersection dut(std::move(orig));
+  EXPECT_EQ(dut.num_elements(), 2);
+  EXPECT_EQ(dut.ambient_dimension(), 2);
+
+  // The old set is in a valid but unspecified state. For convenience we'll
+  // assert that it's empty, but that's not the only valid implementation,
+  // just the one we happen to currently use.
+  EXPECT_EQ(orig.num_elements(), 0);
+  EXPECT_EQ(orig.ambient_dimension(), 0);
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(IntersectionTest, TwoBoxes) {
   HPolyhedron H1 = HPolyhedron::MakeBox(Vector2d{0, 0}, Vector2d{2, 2});
   HPolyhedron H2 = HPolyhedron::MakeBox(Vector2d{1, -1}, Vector2d{3, 1});

geometry/optimization/test/minkowski_sum_test.cc

@@ -63,6 +63,23 @@ GTEST_TEST(MinkowskiSumTest, DefaultCtor) {
   EXPECT_FALSE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
 }
 
+GTEST_TEST(MinkowskiSumTest, Move) {
+  const Point P1(Vector2d{1.2, 3.4}), P2(Vector2d{5.6, 7.8});
+  MinkowskiSum orig(P1, P2);
+
+  // A move-constructed MinkowskiSum takes over the original data.
+  MinkowskiSum dut(std::move(orig));
+  EXPECT_EQ(dut.num_terms(), 2);
+  EXPECT_EQ(dut.ambient_dimension(), 2);
+
+  // The old set is in a valid but unspecified state. For convenience we'll
+  // assert that it's empty, but that's not the only valid implementation,
+  // just the one we happen to currently use.
+  EXPECT_EQ(orig.num_terms(), 0);
+  EXPECT_EQ(orig.ambient_dimension(), 0);
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(MinkowskiSumTest, FromSceneGraph) {
   const RigidTransformd X_WG{math::RollPitchYawd(.1, .2, 3),
                              Vector3d{.5, .87, .1}};

geometry/optimization/test/point_test.cc

@@ -71,6 +71,20 @@ GTEST_TEST(PointTest, DefaultCtor) {
   EXPECT_NO_THROW(P.set_x(Eigen::VectorXd::Zero(0)));
 }
 
+GTEST_TEST(PointTest, Move) {
+  const Vector3d p_W{1.2, 4.5, -2.8};
+  Point orig(p_W);
+
+  // A move-constructed Point takes over the original data.
+  Point dut(std::move(orig));
+  EXPECT_EQ(dut.ambient_dimension(), 3);
+  EXPECT_TRUE(CompareMatrices(dut.x(), p_W));
+
+  // The old Point is in a valid but unspecified state.
+  EXPECT_EQ(orig.x().size(), orig.ambient_dimension());
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(PointTest, FromSceneGraphTest) {
   Vector3d p_W{1.2, 4.5, -2.8};
 

geometry/optimization/test/spectrahedron_test.cc

@@ -210,6 +210,34 @@ GTEST_TEST(SpectrahedronTest, TrivialSdp1) {
   }
 }
 
+GTEST_TEST(SpectrahedronTest, Move) {
+  auto make_sdp = []() {
+    auto sdp = std::make_unique<MathematicalProgram>();
+    sdp->AddPositiveSemidefiniteConstraint(
+        sdp->NewSymmetricContinuousVariables<3>());
+    return sdp;
+  };
+  Spectrahedron orig(*make_sdp());
+  EXPECT_EQ(orig.ambient_dimension(), 6);
+
+  // A move-constructed Spectrahedron takes over the original data.
+  Spectrahedron dut(std::move(orig));
+  EXPECT_EQ(dut.ambient_dimension(), 6);
+  EXPECT_NO_THROW(dut.Clone());
+  // In particular, make sure that the SDP is still intact by checking how many
+  // constraints the spectrahedron emits.
+  MathematicalProgram constraints;
+  EXPECT_NO_THROW(dut.AddPointInSetConstraints(
+      &constraints, constraints.NewContinuousVariables<6>()));
+  EXPECT_EQ(constraints.positive_semidefinite_constraints().size(), 1);
+
+  // The old set is in a valid but unspecified state. For convenience we'll
+  // assert that it's empty, but that's not the only valid implementation,
+  // just the one we happen to currently use.
+  EXPECT_EQ(orig.ambient_dimension(), 0);
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 }  // namespace
 }  // namespace optimization
 }  // namespace geometry

geometry/optimization/test/vpolytope_test.cc

@@ -78,6 +78,24 @@ GTEST_TEST(VPolytopeTest, DefaultCtor) {
   EXPECT_FALSE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
 }
 
+GTEST_TEST(VPolytopeTest, Move) {
+  Eigen::Matrix<double, 2, 3> triangle;
+  // clang-format off
+  triangle <<  2, 4, 3,
+              -1, 2, 5;
+  // clang-format on
+  VPolytope orig(triangle);
+
+  // A move-constructed VPolytope takes over the original data.
+  VPolytope dut(std::move(orig));
+  EXPECT_EQ(dut.ambient_dimension(), 2);
+  EXPECT_TRUE(CompareMatrices(dut.vertices(), triangle));
+
+  // The old VPolytope is in a valid but unspecified state.
+  EXPECT_EQ(orig.vertices().rows(), orig.ambient_dimension());
+  EXPECT_NO_THROW(orig.Clone());
+}
+
 GTEST_TEST(VPolytopeTest, UnitBoxTest) {
   VPolytope V = VPolytope::MakeUnitBox(3);
   EXPECT_EQ(V.ambient_dimension(), 3);