Add QCQP factor graph conversion

gtsam/constrained/QcqpProblem.h

@@ -17,14 +17,108 @@
 
 #pragma once
 
+#include <gtsam/base/Manifold.h>
 #include <gtsam/constrained/ConstrainedOptProblem.h>
 #include <gtsam/constrained/LinearConstraint.h>
 #include <gtsam/constrained/QpCost.h>
 #include <gtsam/constrained/QuadraticConstraint.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
+#include <stdexcept>
+#include <type_traits>
+#include <utility>
+
 namespace gtsam {
 
+namespace internal {
+
+template <typename T, int D, typename = void>
+struct HasQcqpVariableTraits : std::false_type {};
+
+template <typename T, int D>
+struct HasQcqpVariableTraits<
+    T, D,
+    std::void_t<
+        decltype(traits<T>::template QcqpValue<D>(std::declval<T>())),
+        decltype(traits<T>::template QcqpConstraints<D>())>>
+    : std::true_type {};
+
+}  // namespace internal
+
+/**
+ * Insert one matrix-valued QCQP variable using traits<T>::QcqpValue<D>.
+ *
+ * D is the number of columns in the matrix-valued QCQP variable. D=1 stores
+ * vector QCQPs as r-by-1 matrices; larger D values store matrix-valued QCQP
+ * variables without changing the row-space cost matrices.
+ */
+template <typename T, int D = 1>
+void InsertQcqpValue(Key key, const T& value, Values* qcqpValues) {
+  static_assert(internal::HasQcqpVariableTraits<T, D>::value,
+                "InsertQcqpValue requires traits<T>::QcqpValue<D> and "
+                "traits<T>::QcqpConstraints<D>.");
+  if (!qcqpValues) {
+    throw std::invalid_argument("InsertQcqpValue: qcqpValues is null.");
+  }
+
+  const Matrix qcqpValue = traits<T>::template QcqpValue<D>(value);
+  if (qcqpValues->exists(key)) {
+    const Matrix existing = qcqpValues->at<Matrix>(key);
+    if (existing.rows() != qcqpValue.rows() ||
+        existing.cols() != qcqpValue.cols()) {
+      throw std::invalid_argument(
+          "InsertQcqpValue: inconsistent QCQP variable dimensions.");
+    }
+    return;
+  }
+  qcqpValues->insert(key, qcqpValue);
+}
+
+/**
+ * Insert QCQP equality constraints for one variable, skipping duplicate keys.
+ */
+template <typename T, int D = 1>
+void InsertQcqpConstraints(Key key, NonlinearEqualityConstraints* constraints) {
+  static_assert(internal::HasQcqpVariableTraits<T, D>::value,
+                "InsertQcqpConstraints requires traits<T>::QcqpValue<D> and "
+                "traits<T>::QcqpConstraints<D>.");
+  if (!constraints) {
+    throw std::invalid_argument("InsertQcqpConstraints: constraints is null.");
+  }
+
+  for (const auto& factor : *constraints) {
+    if (factor && factor->size() == 1 && factor->keys().front() == key) {
+      return;
+    }
+  }
+
+  for (const auto& [A, b] : traits<T>::template QcqpConstraints<D>()) {
+    constraints->push_back(
+        QuadraticConstraint::Equal(key, A, b).createEqualityFactor());
+  }
+}
+
+/**
+ * Project matrix-form QCQP variables back into typed values: scan
+ * `qcqpValues` and apply `traits<T>::FromQcqpValue<D>` to each slice whose
+ * row dim matches `T`. Slices with other row dims are skipped, so
+ * mixed-type graphs are safe.
+ */
+template <typename T, int D>
+std::vector<std::pair<Key, T>> ExtractQcqpValues(const Values& qcqpValues) {
+  static_assert(internal::HasQcqpVariableTraits<T, D>::value,
+                "ExtractQcqpValues requires traits<T>::QcqpValue<D>, "
+                "QcqpConstraints<D>, and FromQcqpValue<D>.");
+  constexpr int expectedRows = T::LieAlgebra::RowsAtCompileTime;
+  std::vector<std::pair<Key, T>> out;
+  for (const auto& [key, M] : qcqpValues.extract<Matrix>()) {
+    if (M.rows() == expectedRows) {
+      out.emplace_back(key, traits<T>::template FromQcqpValue<D>(M));
+    }
+  }
+  return out;
+}
+
 /**
  * Thin constrained optimization problem for QCQPs over Vector or Matrix values.
  */
@@ -48,6 +142,16 @@ class GTSAM_EXPORT QcqpProblem : public ConstrainedOptProblem {
               const NonlinearInequalityConstraints& ineqConstraints)
       : Base(costs, eqConstraints, ineqConstraints) {}
 
+  /** Convert a supported nonlinear factor graph into QCQP costs/constraints. */
+  explicit QcqpProblem(const NonlinearFactorGraph& graph,
+                       size_t columnDimension = 1) {
+    for (const auto& factor : graph) {
+      if (factor) {
+        factor->qcqpFactors(&costs_, &eqConstraints_, columnDimension);
+      }
+    }
+  }
+
   /** Add a quadratic cost. */
   void addCost(const QpCost& cost) { costs_.emplace_shared<QpCost>(cost); }
 

gtsam/constrained/constrained.md

@@ -39,12 +39,20 @@ It includes classes for representing constraints, building constrained problems,
 - [`QcqpProblem`](doc/QcqpProblem.ipynb): Holds quadratic costs and linear/quadratic constraints over vector or matrix variables.
 - [`QpCost`](doc/QcqpProblem.ipynb): Also used for QCQP objectives; `QpCost(keys, Q, columnDim)` creates a pure row-space quadratic cost $\frac{1}{2}\sum_{ij}\operatorname{tr}(X_i^\top Q_{ij}X_j)$ over vectors or matrices $X_i \in \mathbb{R}^{r_i \times d}$.
 - [`QuadraticConstraint`](doc/QcqpProblem.ipynb): Scalar quadratic constraint $\operatorname{tr}(X^\top A X) \sim b$, where $\sim$ is equal, less-equal, or greater-equal.
+- `QcqpProblem(graph, columnDim)`: Opt-in conversion hook for supported nonlinear factors that can populate `QpCost` objectives and `QuadraticConstraint` equalities over matrix-valued QCQP variables.
+- `InsertQcqpValue<T, D>` and `InsertQcqpConstraints<T, D>`: Helpers for inserting supported QCQP variable values and their equality constraints.
 
 The leading factor of `1/2` in row-space `QpCost` construction is intentional:
 it follows GTSAM's standard factor-error convention. To represent a QCQP
 objective written without the `1/2`, pass twice the row-space `Q` blocks to
 `QpCost`.
 
+The current factor-graph conversion example is intentionally narrow:
+`FrobeniusBetweenFactor<T>` can form generic Frobenius QCQP costs for supported
+matrix Lie groups, but each variable type must provide its own QCQP value and
+constraint traits. `Rot2` currently provides those traits for `D=1`, `D=2`, and
+`D=3`. Unsupported factors throw from `NonlinearFactor::qcqpFactors`.
+
 ## Optimizers
 
 - [`ConstrainedOptimizerParams`](doc/ConstrainedOptimizer.ipynb), [`ConstrainedOptimizerState`](doc/ConstrainedOptimizer.ipynb), [`ConstrainedOptimizer`](doc/ConstrainedOptimizer.ipynb): Shared base interfaces and iteration state for constrained solvers.

gtsam/constrained/doc/QcqpProblem.ipynb

@@ -9,7 +9,7 @@
     "\n",
     "## Overview\n",
     "\n",
-    "`QcqpProblem` represents a quadratically constrained quadratic program over vector or matrix variables. It uses `QpCost` for quadratic objectives, `LinearConstraint` and `QuadraticConstraint` for scalar constraints, and the standard `ConstrainedOptProblem` interface for evaluation and optimization."
+    "`QcqpProblem` represents a quadratically constrained quadratic program over vector or matrix variables. It uses `QpCost` for quadratic objectives, `LinearConstraint` and `QuadraticConstraint` for scalar constraints, and the standard `ConstrainedOptProblem` interface for evaluation and optimization. It also includes a narrow opt-in conversion path for supported nonlinear factor graphs."
    ]
   },
   {
@@ -78,7 +78,9 @@
     "- `QpCost(keys, Q, columnDim)` stores a row-space quadratic objective $\\tfrac{1}{2}\\sum_{ij}\\operatorname{tr}(X_i^\\top Q_{ij}X_j)$ and linearizes exactly to a `HessianFactor`.\n",
     "- The leading $\\tfrac{1}{2}$ follows GTSAM's factor-error convention. To represent a QCQP objective written without that factor, pass twice the row-space $Q$ blocks.\n",
     "- `QuadraticConstraint` stores a scalar quadratic relation $\\operatorname{tr}(X^\\top A X)-b$ with equal, less-equal, or greater-equal sense.\n",
-    "- Variables are stored in `Values` as direct `Vector` or dynamic `Matrix` entries."
+    "- Variables are stored in `Values` as direct `Vector` or dynamic `Matrix` entries.\n",
+    "- `QcqpProblem(graph, columnDim)` asks supported nonlinear factors to contribute `QpCost` and equality constraints through `NonlinearFactor::qcqpFactors`.\n",
+    "- `FrobeniusBetweenFactor<T>` forms generic Frobenius QCQP costs for supported matrix Lie groups, while each variable type provides its own QCQP value and constraint traits. `Rot2` currently provides those traits for `D=1`, `D=2`, and `D=3`."
    ]
   },
   {
@@ -106,9 +108,11 @@
     "## Key C++ API\n",
     "\n",
     "- `QcqpProblem(costs, equalityConstraints)` or `QcqpProblem(costs, equalityConstraints, inequalityConstraints)`\n",
+    "- `QcqpProblem(graph, columnDim)` for supported nonlinear factor graph conversions\n",
     "- `QcqpProblem::addConstraint(LinearConstraint)` and `QcqpProblem::addConstraint(QuadraticConstraint)`\n",
     "- `QpCost(keys, Q, columnDim)` for row-space quadratic objectives; omit `columnDim` for vector variables\n",
     "- `QuadraticConstraint::Equal(key, A, b)`, `QuadraticConstraint::LessEqual(key, A, b)`, and `QuadraticConstraint::GreaterEqual(key, A, b)`\n",
+    "- `InsertQcqpValue<T, D>(key, value, &qcqpValues)` and `InsertQcqpConstraints<T, D>(key, &constraints)` for supported QCQP variable values\n",
     "- inherited `ConstrainedOptProblem` accessors such as `costs()`, `eConstraints()`, and `evaluate(values)`"
    ]
   },
@@ -138,6 +142,26 @@
     "values.insert(x, Vector2(1.0, 0.0));\n",
     "\n",
     "auto [cost, eqViolation, ineqViolation] = problem.evaluate(values);\n",
+    "```\n",
+    "\n",
+    "A small supported SLAM-style conversion can be built from `FrobeniusBetweenFactor<Rot2>`:\n",
+    "\n",
+    "```cpp\n",
+    "#include <gtsam/constrained/QcqpProblem.h>\n",
+    "#include <gtsam/slam/FrobeniusFactor.h>\n",
+    "\n",
+    "Key x1 = Symbol('x', 1), x2 = Symbol('x', 2);\n",
+    "NonlinearFactorGraph graph;\n",
+    "graph.emplace_shared<FrobeniusBetweenFactor<Rot2>>(\n",
+    "    x1, x2, Rot2::fromAngle(0.3));\n",
+    "\n",
+    "QcqpProblem qcqpProblem(graph, 1);\n",
+    "Values qcqpValues;\n",
+    "InsertQcqpValue<Rot2, 1>(x1, Rot2(), &qcqpValues);\n",
+    "InsertQcqpValue<Rot2, 1>(x2, Rot2::fromAngle(0.3), &qcqpValues);\n",
+    "\n",
+    "auto [qcqpCost, qcqpEqViolation, qcqpIneqViolation] =\n",
+    "    qcqpProblem.evaluate(qcqpValues);\n",
     "```\n"
    ]
   },
@@ -189,7 +213,11 @@
     "## Current Scope\n",
     "\n",
     "- `QcqpProblem` assembles quadratic costs plus linear and quadratic constraints.\n",
-    "- `QpCost` and `QuadraticConstraint` operate on direct vector or matrix values already inserted into `Values`."
+    "- `QpCost` and `QuadraticConstraint` operate on direct vector or matrix values already inserted into `Values`.\n",
+    "- Only factors that override `qcqpFactors()` can participate in factor-graph conversion; unsupported factors throw by default.\n",
+    "- `FrobeniusBetweenFactor<T>` contributes generic Frobenius costs when the variable type provides QCQP value and constraint traits.\n",
+    "- `Rot2` is currently the only type with QCQP variable traits for `D=1`, `D=2`, and `D=3`.\n",
+    "- Matrix-valued QCQP variables are stored in `Values` as dynamic `Matrix` entries, not as their original manifold types."
    ]
   },
   {