More efficient AffineHull computation for certain subclasses of ConvexSet. (RobotLocomotion#21828)

Includes shortcuts for AffineBall, AffineSubspace, CartesianProduct, Hyperellipsoid, Hyperrectangle, and VPolytope. (Note that Point, and sets which are obviously singletons, already had special shortcut logic.)

The user-specified tolerance will now be forwarded to these shortcut methods, potentially taking a different role, depending on the type of the set. The function documentation includes details on exactly how each subclass uses that tolerance.

Author: cohnt

geometry/optimization/affine_ball.cc

@@ -188,6 +188,17 @@ AffineBall::DoToShapeWithPose() const {
       "ToShapeWithPose is not yet supported by AffineBall.");
 }
 
+std::unique_ptr<ConvexSet> AffineBall::DoAffineHullShortcut(
+    std::optional<double> tol) const {
+  Eigen::FullPivHouseholderQR<MatrixXd> qr(B_);
+  if (tol) {
+    qr.setThreshold(tol.value());
+  }
+  MatrixXd basis =
+      qr.matrixQ() * MatrixXd::Identity(ambient_dimension(), qr.rank());
+  return std::make_unique<AffineSubspace>(std::move(basis), center_);
+}
+
 std::pair<VectorX<Variable>, std::vector<Binding<Constraint>>>
 AffineBall::DoAddPointInSetConstraints(
     MathematicalProgram* prog,

geometry/optimization/affine_ball.h

@@ -163,6 +163,9 @@ class AffineBall final : public ConvexSet {
   std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
       const final;
 
+  std::unique_ptr<ConvexSet> DoAffineHullShortcut(
+      std::optional<double> tol) const final;
+
   void CheckInvariants() const;
 
   double DoCalcVolume() const final;

geometry/optimization/affine_subspace.cc

@@ -1,7 +1,12 @@
 #include "drake/geometry/optimization/affine_subspace.h"
 
 #include "drake/common/is_approx_equal_abstol.h"
+#include "drake/geometry/optimization/affine_ball.h"
+#include "drake/geometry/optimization/cartesian_product.h"
+#include "drake/geometry/optimization/hyperellipsoid.h"
+#include "drake/geometry/optimization/hyperrectangle.h"
 #include "drake/geometry/optimization/point.h"
+#include "drake/geometry/optimization/vpolytope.h"
 #include "drake/solvers/solve.h"
 
 namespace drake {
@@ -34,8 +39,11 @@ AffineSubspace::AffineSubspace(const Eigen::Ref<const MatrixXd>& basis,
   }
 }
 
-AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
+AffineSubspace::AffineSubspace(const ConvexSet& set, std::optional<double> tol)
     : ConvexSet(0, true) {
+  if (tol) {
+    DRAKE_THROW_UNLESS(tol >= 0);
+  }
   // If the set is clearly a singleton, we can easily compute its affine hull.
   const auto singleton_maybe = set.MaybeGetPoint();
   if (singleton_maybe.has_value()) {
@@ -45,9 +53,26 @@ AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
     return;
   }
 
-  // If the set is not clearly a singleton, we find a feasible point and
-  // iteratively compute a basis of the affine hull. If no feasible point
-  // exists, the set is empty, so we throw an error.
+  // If the set is of a ConvexSet subclass that has an efficient algorithm for
+  // computing the affine hull, we use it. Otherwise, we use the generic
+  // iterative approach.
+  std::unique_ptr<ConvexSet> shortcut = ConvexSet::AffineHullShortcut(set, tol);
+  if (shortcut != nullptr) {
+    // This downcast is per the post-condition of the AffineHullShortcut API.
+    AffineSubspace* downcast = dynamic_cast<AffineSubspace*>(shortcut.get());
+    DRAKE_THROW_UNLESS(downcast != nullptr);
+    *this = std::move(*downcast);
+    return;
+  }
+
+  // If the set is not clearly a singleton and there's no obviously more
+  // efficient algorithm based on the type of the set, we find a feasible point
+  // and iteratively compute a basis of the affine hull. If the numerical
+  // tolerance was not specified, we use a reasonable default.
+  if (!tol) {
+    tol = 1e-12;
+  }
+  // If no feasible point exists, the set is empty, so we throw an error.
   const auto translation_maybe = set.MaybeGetFeasiblePoint();
   if (!translation_maybe.has_value()) {
     throw std::runtime_error(
@@ -138,7 +163,7 @@ AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
                       result.get_solution_result()));
     }
 
-    if (result.get_optimal_cost() < -tol) {
+    if (result.get_optimal_cost() < -tol.value()) {
       // x is in the affine hull, and is added to the basis.
       VectorXd new_basis_vector = result.GetSolution(x);
       basis.col(affine_dimension++) =
@@ -284,6 +309,11 @@ AffineSubspace::DoToShapeWithPose() const {
       "ToShapeWithPose is not supported by AffineSubspace.");
 }
 
+std::unique_ptr<ConvexSet> AffineSubspace::DoAffineHullShortcut(
+    std::optional<double>) const {
+  return std::make_unique<AffineSubspace>(*this);
+}
+
 double AffineSubspace::DoCalcVolume() const {
   if (AffineDimension() < ambient_dimension()) {
     // An AffineSubspace has zero volume if it has a lower affine dimension than

geometry/optimization/affine_subspace.h

@@ -43,19 +43,41 @@ class AffineSubspace final : public ConvexSet {
                           const Eigen::Ref<const Eigen::VectorXd>& translation);
 
   /** Constructs an affine subspace as the affine hull of another convex set.
-  This is done by finding a feasible point in the set, and then iteratively
-  computing feasible vectors until we have a basis that spans the set. If you
-  pass in a convex set whose points are matrix-valued (e.g. a Spectrahedron),
-  then the affine subspace will work over a flattened representation of those
-  coordinates. (So a Spectrahedron with n-by-n matrices will output an
-  AffineSubspace with ambient dimension (n * (n+1)) / 2.)
+  The generic approach is to find a feasible point in the set, and then
+  iteratively compute feasible vectors until we have a basis that spans the set.
+  If you pass in a convex set whose points are matrix-valued (e.g. a
+  Spectrahedron), then the affine subspace will work over a flattened
+  representation of those coordinates. (So a Spectrahedron with n-by-n matrices
+  will output an AffineSubspace with ambient dimension (n * (n+1)) / 2.)
 
   `tol` sets the numerical precision of the computation. For each dimension, a
   pair of feasible points are constructed, so as to maximize the displacement in
   that dimension. If their displacement along that dimension is larger than tol,
   then the vector connecting the points is added as a basis vector.
-  @pre !set.IsEmpty() */
-  explicit AffineSubspace(const ConvexSet& set, double tol = 1e-12);
+
+  @throws std::exception if `set` is empty.
+  @throws std::exception if `tol < 0`.
+
+  For several subclasses of ConvexSet, there is a closed-form computation (or
+  more efficient numerical computation) that is preferred.
+  - AffineBall: Can be computed via a rank-revealing decomposition; `tol` is
+  used as the numerical tolerance for the rank of the matrix. Pass
+  `std::nullopt` for `tol` to use Eigen's automatic tolerance computation.
+  - AffineSubspace: Equivalent to the copy-constructor; `tol` is ignored.
+  - CartesianProduct: Can compute the affine hull of each factor individually;
+  `tol` is propagated to the constituent calls. (This is not done if the
+  Cartesian product has an associated affine transformation.)
+  - Hyperellipsoid: Always equal to the whole ambient space; `tol` is ignored.
+  - Hyperrectangle: Can be computed in closed-form; `tol` has the same meaning
+  as in the generic affine hull computation.
+  - Point: Can be computed in closed-form; `tol` is ignored. This also
+  encompasses sets which are obviously a singleton point, as determined via
+  MaybeGetPoint.
+  - VPolytope: Can be computed via a singular value decomposition; `tol` is
+  used as the numerical tolerance for the rank of the matrix. Pass
+  `std::nullopt` for `tol` to use Eigen's automatic tolerance computation. */
+  explicit AffineSubspace(const ConvexSet& set,
+                          std::optional<double> tol = std::nullopt);
 
   ~AffineSubspace() final;
 
@@ -160,6 +182,9 @@ class AffineSubspace final : public ConvexSet {
   std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
       const final;
 
+  std::unique_ptr<ConvexSet> DoAffineHullShortcut(
+      std::optional<double>) const final;
+
   double DoCalcVolume() const final;
 
   std::vector<std::optional<double>> DoProjectionShortcut(

geometry/optimization/cartesian_product.cc

@@ -4,6 +4,7 @@
 
 #include <fmt/format.h>
 
+#include "drake/geometry/optimization/affine_subspace.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
 #include "drake/geometry/optimization/hyperellipsoid.h"
 #include "drake/solvers/solve.h"
@@ -362,6 +363,38 @@ CartesianProduct::DoToShapeWithPose() const {
       "ToShapeWithPose is not implemented yet for CartesianProduct.");
 }
 
+std::unique_ptr<ConvexSet> CartesianProduct::DoAffineHullShortcut(
+    std::optional<double> tol) const {
+  // TODO(cohnt): Support affine transformations of Cartesian products. For now,
+  // we just return std::nullopt and use the generic affine hull computation.
+  if (A_ != std::nullopt || b_ != std::nullopt) {
+    return nullptr;
+  }
+
+  // The basis will be a block diagonal matrix, whose blocks correspond to the
+  // bases of the affine subspace of each factor. Not all blocks will be square,
+  // and some of the columns on the right will be skipped, since the affine hull
+  // may be a proper subspace.
+  MatrixXd basis = MatrixXd::Zero(ambient_dimension(), ambient_dimension());
+  // The translation will be a vector, concatenating all of the translations of
+  // each factor. Zero-initialization is not needed, since all entries will be
+  // overwritten in the following loop.
+  VectorXd translation(ambient_dimension());
+  int current_dimension = 0;
+  int num_basis_vectors = 0;
+  for (int i = 0; i < num_factors(); ++i) {
+    AffineSubspace a(factor(i), tol);
+    basis.block(current_dimension, num_basis_vectors, a.ambient_dimension(),
+                a.AffineDimension()) = a.basis();
+    translation.segment(current_dimension, a.ambient_dimension()) =
+        a.translation();
+    current_dimension += a.ambient_dimension();
+    num_basis_vectors += a.AffineDimension();
+  }
+  return std::make_unique<AffineSubspace>(basis.leftCols(num_basis_vectors),
+                                          std::move(translation));
+}
+
 double CartesianProduct::DoCalcVolume() const {
   DRAKE_DEMAND(sets_.size() > 0);
   double volume = 1.0;

geometry/optimization/cartesian_product.h

@@ -135,6 +135,9 @@ class CartesianProduct final : public ConvexSet {
   std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
       const final;
 
+  std::unique_ptr<ConvexSet> DoAffineHullShortcut(
+      std::optional<double> tol) const final;
+
   // The member variables are not const in order to support move semantics.
   ConvexSets sets_{};
 

geometry/optimization/convex_set.cc

@@ -444,6 +444,16 @@ double ConvexSet::DoCalcVolume() const {
                   NiceTypeName::Get(*this)));
 }
 
+std::unique_ptr<ConvexSet> ConvexSet::AffineHullShortcut(
+    const ConvexSet& self, std::optional<double> tol) {
+  return self.DoAffineHullShortcut(tol);
+}
+
+std::unique_ptr<ConvexSet> ConvexSet::DoAffineHullShortcut(
+    std::optional<double>) const {
+  return nullptr;
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/convex_set.h

@@ -453,6 +453,22 @@ class ConvexSet {
       solvers::MathematicalProgram* prog, const ConvexSet& set,
       std::vector<solvers::Binding<solvers::Constraint>>* constraints) const;
 
+  /** When there is a more efficient strategy to compute the affine hull of this
+  set, returns affine hull as an AffineSubspace. When no efficient conversion
+  exists, returns null. The default base class implementation returns null. This
+  method is used by the AffineSubspace constructor to short-circuit the generic
+  iterative approach. (This function is static to allow calling it from the
+  AffineSubspace constructor, but is conceptially a normal member function.)
+  The return type is ConvexSet to avoid a forward declaration; any non-null
+  result must always have the AffineSubspace as its runtime type. */
+  static std::unique_ptr<ConvexSet> AffineHullShortcut(
+      const ConvexSet& self, std::optional<double> tol);
+
+  /** NVI implementation of DoAffineHullShortcut, which trivially returns null.
+  Derived classes that have efficient algorithms should override this method. */
+  virtual std::unique_ptr<ConvexSet> DoAffineHullShortcut(
+      std::optional<double> tol) const;
+
  private:
   /** Generic implementation for IsBounded() -- applicable for all convex sets.
   @pre ambient_dimension() >= 0 */

geometry/optimization/hyperellipsoid.cc

@@ -6,6 +6,7 @@
 #include <Eigen/Eigenvalues>
 #include <fmt/format.h>
 
+#include "drake/geometry/optimization/affine_subspace.h"
 #include "drake/math/matrix_util.h"
 #include "drake/math/rotation_matrix.h"
 #include "drake/solvers/choose_best_solver.h"
@@ -396,6 +397,16 @@ void Hyperellipsoid::ImplementGeometry(const Sphere& sphere, void* data) {
   *A = Eigen::Matrix3d::Identity() / sphere.radius();
 }
 
+std::unique_ptr<ConvexSet> Hyperellipsoid::DoAffineHullShortcut(
+    std::optional<double> tol) const {
+  // Hyperellipsoids are always positive volume, so we can trivially construct
+  // their affine hull as the whole vector space.
+  unused(tol);
+  const int n = ambient_dimension();
+  return std::make_unique<AffineSubspace>(Eigen::MatrixXd::Identity(n, n),
+                                          Eigen::VectorXd::Zero(n));
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/hyperellipsoid.h

@@ -177,6 +177,9 @@ class Hyperellipsoid final : public ConvexSet, private ShapeReifier {
   std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
       const final;
 
+  std::unique_ptr<ConvexSet> DoAffineHullShortcut(
+      std::optional<double> tol) const final;
+
   double DoCalcVolume() const final;
 
   void CheckInvariants() const;

geometry/optimization/hyperrectangle.cc

@@ -13,6 +13,7 @@
 #include <Eigen/Eigenvalues>
 #include <fmt/format.h>
 
+#include "drake/geometry/optimization/affine_subspace.h"
 #include "drake/geometry/optimization/convex_set.h"
 #include "drake/solvers/solve.h"
 
@@ -208,6 +209,24 @@ Hyperrectangle::DoToShapeWithPose() const {
                         math::RigidTransformd(Center()));
 }
 
+std::unique_ptr<ConvexSet> Hyperrectangle::DoAffineHullShortcut(
+    std::optional<double> tol) const {
+  MatrixXd basis = MatrixXd::Zero(ambient_dimension(), ambient_dimension());
+  int current_dimension = 0;
+  int num_basis_vectors = 0;
+  for (int i = 0; i < ambient_dimension(); ++i) {
+    // If the numerical tolerance was not specified, we use a reasonable
+    // default.
+    if (ub_[i] - lb_[i] > (tol ? tol.value() : 1e-12)) {
+      basis(current_dimension, num_basis_vectors) = 1;
+      ++num_basis_vectors;
+    }
+    ++current_dimension;
+  }
+  return std::make_unique<AffineSubspace>(basis.leftCols(num_basis_vectors),
+                                          lb_);
+}
+
 double Hyperrectangle::DoCalcVolume() const {
   return (ub_ - lb_).prod();
 }

geometry/optimization/hyperrectangle.h

@@ -111,6 +111,9 @@ class Hyperrectangle final : public ConvexSet {
   std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
       const final;
 
+  std::unique_ptr<ConvexSet> DoAffineHullShortcut(
+      std::optional<double> tol) const final;
+
   // TODO(Alexandre.Amice) Implement DoProjectionShortcut.
 
   double DoCalcVolume() const final;