diff --git a/gtsam/constrained/QcqpProblem.h b/gtsam/constrained/QcqpProblem.h index a83661a15a..23f95bdbe8 100644 --- a/gtsam/constrained/QcqpProblem.h +++ b/gtsam/constrained/QcqpProblem.h @@ -17,14 +17,108 @@ #pragma once +#include #include #include #include #include #include +#include +#include +#include + namespace gtsam { +namespace internal { + +template +struct HasQcqpVariableTraits : std::false_type {}; + +template +struct HasQcqpVariableTraits< + T, D, + std::void_t< + decltype(traits::template QcqpValue(std::declval())), + decltype(traits::template QcqpConstraints())>> + : std::true_type {}; + +} // namespace internal + +/** + * Insert one matrix-valued QCQP variable using traits::QcqpValue. + * + * 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 +void InsertQcqpValue(Key key, const T& value, Values* qcqpValues) { + static_assert(internal::HasQcqpVariableTraits::value, + "InsertQcqpValue requires traits::QcqpValue and " + "traits::QcqpConstraints."); + if (!qcqpValues) { + throw std::invalid_argument("InsertQcqpValue: qcqpValues is null."); + } + + const Matrix qcqpValue = traits::template QcqpValue(value); + if (qcqpValues->exists(key)) { + const Matrix existing = qcqpValues->at(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 +void InsertQcqpConstraints(Key key, NonlinearEqualityConstraints* constraints) { + static_assert(internal::HasQcqpVariableTraits::value, + "InsertQcqpConstraints requires traits::QcqpValue and " + "traits::QcqpConstraints."); + 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::template QcqpConstraints()) { + constraints->push_back( + QuadraticConstraint::Equal(key, A, b).createEqualityFactor()); + } +} + +/** + * Project matrix-form QCQP variables back into typed values: scan + * `qcqpValues` and apply `traits::FromQcqpValue` to each slice whose + * row dim matches `T`. Slices with other row dims are skipped, so + * mixed-type graphs are safe. + */ +template +std::vector> ExtractQcqpValues(const Values& qcqpValues) { + static_assert(internal::HasQcqpVariableTraits::value, + "ExtractQcqpValues requires traits::QcqpValue, " + "QcqpConstraints, and FromQcqpValue."); + constexpr int expectedRows = T::LieAlgebra::RowsAtCompileTime; + std::vector> out; + for (const auto& [key, M] : qcqpValues.extract()) { + if (M.rows() == expectedRows) { + out.emplace_back(key, traits::template FromQcqpValue(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(cost); } diff --git a/gtsam/constrained/constrained.md b/gtsam/constrained/constrained.md index 20314dbfed..41726aeb06 100644 --- a/gtsam/constrained/constrained.md +++ b/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` and `InsertQcqpConstraints`: 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` 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. diff --git a/gtsam/constrained/doc/QcqpProblem.ipynb b/gtsam/constrained/doc/QcqpProblem.ipynb index 8a1e761076..ada61dfae1 100644 --- a/gtsam/constrained/doc/QcqpProblem.ipynb +++ b/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` 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(key, value, &qcqpValues)` and `InsertQcqpConstraints(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`:\n", + "\n", + "```cpp\n", + "#include \n", + "#include \n", + "\n", + "Key x1 = Symbol('x', 1), x2 = Symbol('x', 2);\n", + "NonlinearFactorGraph graph;\n", + "graph.emplace_shared>(\n", + " x1, x2, Rot2::fromAngle(0.3));\n", + "\n", + "QcqpProblem qcqpProblem(graph, 1);\n", + "Values qcqpValues;\n", + "InsertQcqpValue(x1, Rot2(), &qcqpValues);\n", + "InsertQcqpValue(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` 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." ] }, { diff --git a/gtsam/constrained/tests/testQcqpProblem.cpp b/gtsam/constrained/tests/testQcqpProblem.cpp index b715b5ba31..bfa7366a93 100644 --- a/gtsam/constrained/tests/testQcqpProblem.cpp +++ b/gtsam/constrained/tests/testQcqpProblem.cpp @@ -22,9 +22,15 @@ #include #include #include +#include +#include #include #include +#include +#include +#include +#include #include using namespace gtsam; @@ -309,6 +315,438 @@ TEST(QcqpProblem, OptimizeAugmentedLagrangianMixedConstraints) { } } // namespace QcqpProblemFixture +/* ************************************************************************* */ +namespace QcqpSingleFactorFixture { + +const Key x0 = Symbol('x', 0); +const Key x1 = Symbol('x', 1); + +Values SingleFactorManifoldValues() { + Values values; + values.insert(x0, Rot2::fromAngle(0.3)); + values.insert(x1, Rot2::fromAngle(-0.2)); + return values; +} + +Values SingleFactorQcqpValues() { + Values values; + InsertQcqpValue(x0, Rot2::fromAngle(0.3), &values); + InsertQcqpValue(x1, Rot2::fromAngle(-0.2), &values); + return values; +} + +bool ThrowsMissingQcqpTraits(const NonlinearFactorGraph& graph) { + try { + QcqpProblem problem(graph); + } catch (const std::runtime_error& exception) { + return std::string(exception.what()).find("QCQP variable traits") != + std::string::npos; + } + return false; +} + +// Verifies unsupported factors still reject QCQP conversion. +TEST(QcqpProblem, UnsupportedFactorThrows) { + NonlinearFactorGraph graph; + graph.emplace_shared>(x0, x1, Rot2::fromAngle(0.1)); + + CHECK_EXCEPTION({ QcqpProblem problem(graph); }, std::runtime_error); +} + +// Verifies missing QCQP variable traits produce a generic conversion error. +TEST(QcqpProblem, MissingQcqpTraitsThrows) { + NonlinearFactorGraph graph; + graph.emplace_shared>( + x0, x1, SO3::Expmap((Vector3() << 0.1, 0.2, 0.3).finished())); + + EXPECT(ThrowsMissingQcqpTraits(graph)); +} + +// Verifies one Rot2 Frobenius factor converts exactly with D=1 QCQP variables. +TEST(QcqpProblem, SingleFrobeniusBetweenFactor) { + NonlinearFactorGraph graph; + graph.emplace_shared>(x0, x1, + Rot2::fromAngle(0.4)); + + const Values values = SingleFactorManifoldValues(); + const QcqpProblem problem(graph); + const Values qcqpValues = SingleFactorQcqpValues(); + + EXPECT_DOUBLES_EQUAL(graph.error(values), problem.costs().error(qcqpValues), + 1e-12); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +} // namespace QcqpSingleFactorFixture +/* ************************************************************************* */ +namespace QcqpRingFixture { + +NonlinearFactorGraph RingGraph(size_t numPoses, double delta) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < numPoses; ++i) { + graph.emplace_shared>( + Symbol('x', i), Symbol('x', (i + 1) % numPoses), + Rot2::fromAngle(delta)); + } + return graph; +} + +Values RingValues(size_t numPoses, double delta, double perturbation) { + Values values; + for (size_t i = 0; i < numPoses; ++i) { + values.insert( + Symbol('x', i), + Rot2::fromAngle(i * delta + perturbation * static_cast(i))); + } + return values; +} + +Values RingQcqpValues(size_t numPoses, double delta, double perturbation) { + Values values; + for (size_t i = 0; i < numPoses; ++i) { + InsertQcqpValue( + Symbol('x', i), + Rot2::fromAngle(i * delta + perturbation * static_cast(i)), + &values); + } + return values; +} + +// Verifies a Rot2 ring graph preserves cost and constraints for D=1 variables. +TEST(QcqpProblem, Rot2RingPgo) { + constexpr size_t N = 5; + constexpr double delta = 2.0 * M_PI / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const Values values = RingValues(N, delta, 0.01); + const QcqpProblem problem(graph); + const Values qcqpValues = RingQcqpValues(N, delta, 0.01); + + EXPECT_DOUBLES_EQUAL(graph.error(values), problem.costs().error(qcqpValues), + 1e-12); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +// Verifies the constrained optimizer still reduces a Rot2 QCQP ring problem. +TEST(QcqpProblem, AugmentedLagrangianOptimizerRot2Ring) { + constexpr size_t N = 5; + constexpr double delta = 2.0 * M_PI / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const QcqpProblem problem(graph); + const Values initialValues = RingQcqpValues(N, delta, 0.03); + const double initialCost = problem.costs().error(initialValues); + + auto params = std::make_shared(); + params->maxIterations = 5; + params->initialMuEq = 10.0; + params->muEqIncreaseRate = 5.0; + params->absoluteViolationTolerance = 1e-6; + params->absoluteCostTolerance = 1e-12; + params->verbose = false; + + const AugmentedLagrangianOptimizer optimizer(problem, initialValues, params); + const Values result = optimizer.optimize(); + + EXPECT(problem.eConstraints().violationNorm(result) < 1e-5); + EXPECT(problem.costs().error(result) <= initialCost + 1e-9); +} + +} // namespace QcqpRingFixture +/* ************************************************************************* */ +namespace QcqpRot2VariableFixture { + +const Key x0 = Symbol('x', 0); +const Key x1 = Symbol('x', 1); + +template +Values QcqpValues(const Rot2& R0, const Rot2& R1) { + Values values; + InsertQcqpValue(x0, R0, &values); + InsertQcqpValue(x1, R1, &values); + return values; +} + +template +double DirectQcqpBetweenCost(const Rot2& R0, const Rot2& R1, + const Rot2& measured) { + const Matrix X0 = traits::template QcqpValue(R0); + const Matrix X1 = traits::template QcqpValue(R1); + const Matrix residual = X1 - measured.matrix().transpose() * X0; + return 0.5 * residual.squaredNorm(); +} + +// Verifies the D=2 Rot2 QCQP variable is row-orthonormal and feasible. +TEST(QcqpProblem, Rot2D2QcqpValueConstraints) { + const Rot2 R = Rot2::fromAngle(0.3); + const Matrix X = traits::QcqpValue<2>(R); + LONGS_EQUAL(2, X.rows()); + LONGS_EQUAL(2, X.cols()); + EXPECT(assert_equal(Matrix(Matrix2::Identity()), X * X.transpose(), 1e-12)); + + NonlinearEqualityConstraints constraints; + InsertQcqpConstraints(x0, &constraints); + Values values; + values.insert(x0, X); + + EXPECT_DOUBLES_EQUAL(0.0, constraints.violationNorm(values), 1e-12); +} + +// Verifies the D=3 Rot2 QCQP variable reuses the D=2 row constraints. +TEST(QcqpProblem, Rot2D3QcqpValueConstraints) { + const Rot2 R = Rot2::fromAngle(-0.4); + const Matrix X = traits::QcqpValue<3>(R); + LONGS_EQUAL(2, X.rows()); + LONGS_EQUAL(3, X.cols()); + EXPECT(assert_equal(Matrix(Matrix2::Identity()), X * X.transpose(), 1e-12)); + + const auto constraintsD2 = traits::QcqpConstraints<2>(); + const auto constraintsD3 = traits::QcqpConstraints<3>(); + LONGS_EQUAL(constraintsD2.size(), constraintsD3.size()); + for (size_t i = 0; i < constraintsD2.size(); ++i) { + EXPECT(assert_equal(constraintsD2[i].first, constraintsD3[i].first, 1e-12)); + EXPECT_DOUBLES_EQUAL(constraintsD2[i].second, constraintsD3[i].second, + 1e-12); + } + + NonlinearEqualityConstraints constraints; + InsertQcqpConstraints(x0, &constraints); + Values values; + values.insert(x0, X); + + EXPECT_DOUBLES_EQUAL(0.0, constraints.violationNorm(values), 1e-12); +} + +// Verifies D=2 row-space Frobenius conversion matches the manifold factor. +TEST(QcqpProblem, Rot2FrobeniusBetweenFactorD2) { + const Rot2 measured = Rot2::fromAngle(0.4); + const Rot2 R0 = Rot2::fromAngle(0.3); + const Rot2 R1 = Rot2::fromAngle(-0.2); + + NonlinearFactorGraph graph; + graph.emplace_shared>(x0, x1, measured); + + Values manifoldValues; + manifoldValues.insert(x0, R0); + manifoldValues.insert(x1, R1); + + const QcqpProblem problem(graph, 2); + const Values qcqpValues = QcqpValues<2>(R0, R1); + + EXPECT_DOUBLES_EQUAL(graph.error(manifoldValues), + problem.costs().error(qcqpValues), 1e-12); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +// Verifies D=3 Frobenius conversion matches the manifold factor. +TEST(QcqpProblem, Rot2FrobeniusBetweenFactorD3) { + const Rot2 measured = Rot2::fromAngle(0.4); + const Rot2 R0 = Rot2::fromAngle(0.3); + const Rot2 R1 = Rot2::fromAngle(-0.2); + + NonlinearFactorGraph graph; + graph.emplace_shared>(x0, x1, measured); + + const QcqpProblem problem(graph, 3); + const Values qcqpValues = QcqpValues<3>(R0, R1); + const double qcqpCost = problem.costs().error(qcqpValues); + + Values manifoldValues; + manifoldValues.insert(x0, R0); + manifoldValues.insert(x1, R1); + + EXPECT(std::isfinite(qcqpCost)); + EXPECT_DOUBLES_EQUAL(graph.error(manifoldValues), qcqpCost, 1e-12); + EXPECT_DOUBLES_EQUAL(DirectQcqpBetweenCost<3>(R0, R1, measured), qcqpCost, + 1e-12); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +} // namespace QcqpRot2VariableFixture +/* ************************************************************************* */ +namespace QcqpTraitExtensionsFixture { + +const Key x0 = Symbol('x', 0); +const Key x1 = Symbol('x', 1); + +// Verifies Rot2 QcqpValue at D=4 zero-pads beyond the intrinsic dim. +TEST(QcqpProblem, Rot2QcqpValueD4Padding) { + const Rot2 R = Rot2::fromAngle(0.7); + const Matrix X4 = traits::template QcqpValue<4>(R); + LONGS_EQUAL(2, X4.rows()); + LONGS_EQUAL(4, X4.cols()); + EXPECT(assert_equal(Matrix(R.matrix().transpose()), + Matrix(X4.leftCols<2>()), 1e-12)); + EXPECT(assert_equal(Matrix(Matrix::Zero(2, 2)), + Matrix(X4.rightCols<2>()), 1e-12)); + EXPECT(assert_equal(Matrix(Matrix2::Identity()), X4 * X4.transpose(), 1e-12)); +} + +// Verifies Rot2 matrix-form constraints are equal across D = 2, 3, 5. +TEST(QcqpProblem, Rot2QcqpConstraintsAreDIndependent) { + const auto cs2 = traits::template QcqpConstraints<2>(); + const auto cs3 = traits::template QcqpConstraints<3>(); + const auto cs5 = traits::template QcqpConstraints<5>(); + LONGS_EQUAL(3, cs2.size()); + LONGS_EQUAL(cs2.size(), cs3.size()); + LONGS_EQUAL(cs2.size(), cs5.size()); + for (size_t i = 0; i < cs2.size(); ++i) { + EXPECT(assert_equal(cs2[i].first, cs3[i].first, 1e-12)); + EXPECT(assert_equal(cs2[i].first, cs5[i].first, 1e-12)); + EXPECT_DOUBLES_EQUAL(cs2[i].second, cs3[i].second, 1e-12); + EXPECT_DOUBLES_EQUAL(cs2[i].second, cs5[i].second, 1e-12); + } +} + +// Verifies Rot3 QcqpValue and QcqpConstraints at D=3. +TEST(QcqpProblem, Rot3D3QcqpValueConstraints) { + const Rot3 R = Rot3::Expmap((Vector3() << 0.4, -0.1, 0.7).finished()); + const Matrix X = traits::template QcqpValue<3>(R); + LONGS_EQUAL(3, X.rows()); + LONGS_EQUAL(3, X.cols()); + EXPECT(assert_equal(Matrix(R.matrix().transpose()), X, 1e-12)); + EXPECT(assert_equal(Matrix(Matrix3::Identity()), X * X.transpose(), 1e-12)); + + const auto cs = traits::template QcqpConstraints<3>(); + LONGS_EQUAL(6, cs.size()); + + NonlinearEqualityConstraints constraints; + InsertQcqpConstraints(x0, &constraints); + Values values; + values.insert(x0, X); + EXPECT_DOUBLES_EQUAL(0.0, constraints.violationNorm(values), 1e-12); +} + +// Verifies Rot3 rejects D=2 for QcqpValue and QcqpConstraints. +TEST(QcqpProblem, Rot3D2Throws) { + const Rot3 R = Rot3::Expmap(Vector3::Zero()); + CHECK_EXCEPTION(traits::template QcqpValue<2>(R), + std::invalid_argument); + CHECK_EXCEPTION(traits::template QcqpConstraints<2>(), + std::invalid_argument); +} + +// Verifies FrobeniusBetweenFactor matches the manifold form at K=3. +TEST(QcqpProblem, Rot3FrobeniusBetweenFactorD3) { + const Rot3 measured = Rot3::Expmap((Vector3() << 0.2, 0.1, -0.3).finished()); + const Rot3 R0 = Rot3::Expmap((Vector3() << 0.05, 0.10, 0.15).finished()); + const Rot3 R1 = Rot3::Expmap((Vector3() << 0.10, 0.20, 0.30).finished()); + + NonlinearFactorGraph graph; + auto noise = noiseModel::Isotropic::Sigma(Rot3::dimension, 1.0); + graph.emplace_shared>(x0, x1, measured, noise); + + Values manifoldValues; + manifoldValues.insert(x0, R0); + manifoldValues.insert(x1, R1); + + const QcqpProblem problem(graph, 3); + Values qcqpValues; + InsertQcqpValue(x0, R0, &qcqpValues); + InsertQcqpValue(x1, R1, &qcqpValues); + const double qcqpCost = problem.costs().error(qcqpValues); + + EXPECT(std::isfinite(qcqpCost)); + EXPECT_DOUBLES_EQUAL(graph.error(manifoldValues), qcqpCost, 1e-12); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +// Verifies FrobeniusBetweenFactor matches the manifold form at K=4. +TEST(QcqpProblem, Rot2FrobeniusBetweenFactorD4) { + const Rot2 measured = Rot2::fromAngle(0.4); + const Rot2 R0 = Rot2::fromAngle(0.3); + const Rot2 R1 = Rot2::fromAngle(-0.2); + + NonlinearFactorGraph graph; + auto noise = noiseModel::Isotropic::Sigma(1, 1.0); + graph.emplace_shared>(x0, x1, measured, noise); + + Values manifoldValues; + manifoldValues.insert(x0, R0); + manifoldValues.insert(x1, R1); + + const QcqpProblem problem(graph, 4); + Values qcqpValues; + InsertQcqpValue(x0, R0, &qcqpValues); + InsertQcqpValue(x1, R1, &qcqpValues); + const double qcqpCost = problem.costs().error(qcqpValues); + + EXPECT(std::isfinite(qcqpCost)); + EXPECT_DOUBLES_EQUAL(graph.error(manifoldValues), qcqpCost, 1e-12); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +// Verifies FrobeniusBetweenFactor rejects K=2 at QCQP construction. +TEST(QcqpProblem, Rot3FrobeniusBetweenFactorK2Rejected) { + NonlinearFactorGraph graph; + auto noise = noiseModel::Isotropic::Sigma(Rot3::dimension, 1.0); + graph.emplace_shared>( + x0, x1, Rot3::Expmap(Vector3::Zero()), noise); + CHECK_EXCEPTION({ QcqpProblem problem(graph, /*K=*/2); }, + std::invalid_argument); +} + +} // namespace QcqpTraitExtensionsFixture + +/* ************************************************************************* */ +// Round-trip Rot2 through Insert/ExtractQcqpValues at D=2. +TEST(QcqpProblem, ExtractQcqpValuesRot2) { + Values values; + const Rot2 R0 = Rot2::fromAngle(0.25); + const Rot2 R1 = Rot2::fromAngle(-1.10); + InsertQcqpValue(Symbol('x', 0), R0, &values); + InsertQcqpValue(Symbol('x', 1), R1, &values); + + const auto extracted = ExtractQcqpValues(values); + LONGS_EQUAL(2, extracted.size()); + + Values out; + for (auto& [key, R] : extracted) out.insert(key, R); + EXPECT(assert_equal(R0, out.at(Symbol('x', 0)), 1e-12)); + EXPECT(assert_equal(R1, out.at(Symbol('x', 1)), 1e-12)); +} + +/* ************************************************************************* */ +// Round-trip Rot3 through Insert/ExtractQcqpValues at D=3. +TEST(QcqpProblem, ExtractQcqpValuesRot3) { + Values values; + const Rot3 R0 = Rot3::Rz(0.25); + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.4, 0.7); + InsertQcqpValue(Symbol('x', 0), R0, &values); + InsertQcqpValue(Symbol('x', 1), R1, &values); + + const auto extracted = ExtractQcqpValues(values); + LONGS_EQUAL(2, extracted.size()); + + Values out; + for (auto& [key, R] : extracted) out.insert(key, R); + EXPECT(assert_equal(R0, out.at(Symbol('x', 0)), 1e-12)); + EXPECT(assert_equal(R1, out.at(Symbol('x', 1)), 1e-12)); +} + +/* ************************************************************************* */ +// On a mixed Rot2 + Rot3 Values at D=3, ExtractQcqpValues keeps +// only the row-dim-3 slice. +TEST(QcqpProblem, ExtractQcqpValuesSkipsForeignSlices) { + Values values; + const Rot2 R2 = Rot2::fromAngle(0.3); + const Rot3 R3 = Rot3::Rz(0.5); + InsertQcqpValue(Symbol('a', 0), R2, &values); + InsertQcqpValue(Symbol('a', 1), R2, &values); + InsertQcqpValue(Symbol('b', 0), R3, &values); + + const auto rot3s = ExtractQcqpValues(values); + LONGS_EQUAL(1, rot3s.size()); + EXPECT(rot3s.front().first == Symbol('b', 0)); + EXPECT(assert_equal(R3, rot3s.front().second, 1e-12)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 74ba476038..71d9d7e0f1 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -23,6 +23,9 @@ #include #include +#include +#include +#include namespace gtsam { @@ -248,10 +251,105 @@ namespace gtsam { }; - template <> -struct traits : public internal::MatrixLieGroup {}; +template <> +struct traits : public internal::MatrixLieGroup { + /** + * Return a matrix-valued QCQP variable for Rot2. + * + * D=1 is vectorized SO(2) as a 4-by-1 matrix. + * D=2 returns R' as a 2-by-2 row-orthonormal matrix. + * D>=3 returns [R', 0] as a 2-by-D row-orthonormal matrix. + */ + template + static Matrix QcqpValue(const Rot2& value) { + if constexpr (D == 1) { + const Matrix2 R = value.matrix(); + return Eigen::Map(R.data(), 4, 1); + } else if constexpr (D == 2) { + return value.matrix().transpose(); + } else if constexpr (D >= 3) { + Matrix X = Matrix::Zero(2, D); + X.leftCols<2>() = value.matrix().transpose(); + return X; + } else { + throw std::invalid_argument( + "traits::QcqpValue only supports D=1, D=2, and D>=3."); + } + } + + /** + * Return row-space QCQP equality constraints A, b such that + * trace(X' A X) = b. For D=1 these are the vec(R) SO(2) constraints, + * including orientation. For D>=2 the same 2-by-2 constraints enforce + * row orthonormality. + */ + template + static std::vector> QcqpConstraints() { + if constexpr (D == 1) { + std::vector> constraints; + constraints.reserve(4); + + Matrix A = Matrix::Zero(4, 4); + + A(0, 0) = 1.0; + A(1, 1) = 1.0; + constraints.emplace_back(A, 1.0); + + A.setZero(); + A(2, 2) = 1.0; + A(3, 3) = 1.0; + constraints.emplace_back(A, 1.0); + + A.setZero(); + A(0, 2) = 0.5; + A(2, 0) = 0.5; + A(1, 3) = 0.5; + A(3, 1) = 0.5; + constraints.emplace_back(A, 0.0); + + A.setZero(); + A(0, 3) = 0.5; + A(3, 0) = 0.5; + A(1, 2) = -0.5; + A(2, 1) = -0.5; + constraints.emplace_back(A, 1.0); + + return constraints; + } else if constexpr (D >= 2) { + std::vector> constraints; + constraints.reserve(3); + + Matrix A = Matrix::Zero(2, 2); + A(0, 0) = 1.0; + constraints.emplace_back(A, 1.0); + + A.setZero(); + A(1, 1) = 1.0; + constraints.emplace_back(A, 1.0); + + A.setZero(); + A(0, 1) = 0.5; + A(1, 0) = 0.5; + constraints.emplace_back(A, 0.0); + + return constraints; + } else { + throw std::invalid_argument( + "traits::QcqpConstraints only supports D=1 and D>=2."); + } + } + + /// Project a 2-by-D matrix back to Rot2 via Rot2::ClosestTo on the + /// leading 2-by-2 block. + template + static Rot2 FromQcqpValue(const Matrix& X) { + static_assert(D >= 2, + "traits::FromQcqpValue requires D >= 2."); + return Rot2::ClosestTo(X.template leftCols<2>().transpose()); + } +}; template <> -struct traits : public internal::MatrixLieGroup {}; +struct traits : public traits {}; } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 778c6e1020..e27e3fb93f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -596,9 +596,69 @@ class GTSAM_EXPORT Rot3 : public MatrixLieGroup { const Matrix3& A, OptionalJacobian<3, 9> H = {}); template <> -struct traits : public internal::MatrixLieGroup {}; +struct traits : public internal::MatrixLieGroup { + /** + * Return a matrix-valued QCQP variable for Rot3. + * + * D>=3 returns [R', 0] as a 3-by-D row-orthonormal matrix. + */ + template + static Matrix QcqpValue(const Rot3& value) { + if constexpr (D >= 3) { + Matrix X = Matrix::Zero(3, D); + X.template leftCols<3>() = value.matrix().transpose(); + return X; + } else { + throw std::invalid_argument( + "traits::QcqpValue only supports D>=3."); + } + } + + /** + * Return row-space QCQP equality constraints A, b such that + * trace(X' A X) = b. For D>=3 the same 3-by-3 constraints enforce + * row orthonormality. + */ + template + static std::vector> QcqpConstraints() { + if constexpr (D >= 3) { + std::vector> constraints; + constraints.reserve(6); + + // 3 row-unit-norm: ||row r||^2 = 1. + for (int r = 0; r < 3; ++r) { + Matrix A = Matrix::Zero(3, 3); + A(r, r) = 1.0; + constraints.emplace_back(A, 1.0); + } + + // 3 row-orthogonality: row r1 . row r2 = 0. + for (int r1 = 0; r1 < 3; ++r1) { + for (int r2 = r1 + 1; r2 < 3; ++r2) { + Matrix A = Matrix::Zero(3, 3); + A(r1, r2) = 0.5; + A(r2, r1) = 0.5; + constraints.emplace_back(A, 0.0); + } + } + return constraints; + } else { + throw std::invalid_argument( + "traits::QcqpConstraints only supports D>=3."); + } + } + + /// Project a 3-by-D matrix back to Rot3 via Rot3::ClosestTo on the + /// leading 3-by-3 block. + template + static Rot3 FromQcqpValue(const Matrix& X) { + static_assert(D >= 3, + "traits::FromQcqpValue requires D >= 3."); + return Rot3::ClosestTo(X.template leftCols<3>().transpose()); + } +}; template <> -struct traits : public internal::MatrixLieGroup {}; +struct traits : public traits {}; } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 5eabfac910..820de51961 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -33,6 +33,14 @@ double NonlinearFactor::error(const HybridValues& c) const { return this->error(c.nonlinear()); } +/* ************************************************************************* */ +void NonlinearFactor::qcqpFactors( + NonlinearFactorGraph* /*costs*/, + NonlinearEqualityConstraints* /*constraints*/, + size_t /*columnDimension*/) const { + throw std::runtime_error("NonlinearFactor::qcqpFactors is not implemented"); +} + /* ************************************************************************* */ void NonlinearFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ab03fe2ff8..01014f7957 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -37,6 +37,9 @@ namespace gtsam { +class NonlinearEqualityConstraints; +class NonlinearFactorGraph; + /* ************************************************************************* */ /** These typedefs and aliases will help with making the evaluateError interface @@ -145,6 +148,15 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { virtual std::shared_ptr linearize(const Values& c) const = 0; + /** + * Add this factor's QCQP cost and constraints over matrix-valued QCQP + * variables with the given column dimension. The default implementation + * throws for unsupported factors. + */ + virtual void qcqpFactors(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints, + size_t columnDimension = 1) const; + /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow * for subclasses diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index b28a3a0681..31baee69b4 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -18,12 +18,16 @@ #pragma once +#include +#include #include #include #include #include #include +#include + namespace gtsam { /** @@ -261,6 +265,134 @@ class FrobeniusBetweenFactor : public FrobeniusBetweenFactorNL { if (H1) *H1 = -vec_H_T2hat * T2hat_H_T1_; return error; } + + /** Add this Frobenius between factor as a QCQP cost when traits exist. + * Dispatches on the QCQP variable's column dimension K: K=1 takes the + * vec(R) form, K>=2 takes the matrix form. */ + void qcqpFactors(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints, + size_t columnDimension = 1) const override { + if (columnDimension == 0) { + throw std::invalid_argument( + "FrobeniusBetweenFactor::qcqpFactors: columnDimension must be >= 1"); + } + if (columnDimension == 1) { + qcqpFactorsForVec(costs, constraints); + } else { + qcqpFactorsForMatrix(costs, constraints, columnDimension); + } + } + + private: + static Matrix RightProductMatrix(const Matrix& right) { + constexpr int AmbientDim = N * N; + const Matrix I = Matrix::Identity(N, N); + Matrix result = Matrix::Zero(AmbientDim, AmbientDim); + for (int column = 0; column < N; ++column) { + for (int sourceColumn = 0; sourceColumn < N; ++sourceColumn) { + result.block(column * N, sourceColumn * N, N, N) = + right(sourceColumn, column) * I; + } + } + return result; + } + + /// Vec(R) form (K=1): variable is (N*N)x1; accepts any non-robust + /// Gaussian noise model. + void qcqpFactorsForVec(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints) const { + if constexpr (!internal::HasQcqpVariableTraits::value) { + (void)costs; + (void)constraints; + throw std::runtime_error( + "FrobeniusBetweenFactor::qcqpFactors requires QCQP variable traits " + "for this type and column dimension 1."); + } else { + if (!costs) { + throw std::invalid_argument( + "FrobeniusBetweenFactor::qcqpFactors costs is null"); + } + if (std::dynamic_pointer_cast(this->noiseModel_) || + this->noiseModel_->isConstrained()) { + throw std::runtime_error( + "FrobeniusBetweenFactor::qcqpFactors requires a " + "non-robust quadratic noise model"); + } + + InsertQcqpConstraints(this->key1(), constraints); + InsertQcqpConstraints(this->key2(), constraints); + + constexpr int AmbientDim = N * N; + const Matrix measurement = this->T12_.matrix(); + const Matrix A = RightProductMatrix(measurement); + + Matrix B = Matrix::Zero(AmbientDim, 2 * AmbientDim); + B.block(0, 0, AmbientDim, AmbientDim) = -A; + B.block(0, AmbientDim, AmbientDim, AmbientDim) = + Matrix::Identity(AmbientDim, AmbientDim); + + const Matrix whitenedB = this->noiseModel_->Whiten(B); + const Matrix Q = whitenedB.transpose() * whitenedB; + const SymmetricBlockMatrix blockQ( + std::vector{AmbientDim, AmbientDim}, Q); + costs->push_back(std::make_shared( + KeyVector{this->key1(), this->key2()}, blockQ)); + } + } + + /// Matrix form (K>=N, where N is the variable's intrinsic row dim): + /// variable is N-by-K row-orthonormal; requires an isotropic noise model. + void qcqpFactorsForMatrix(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints, + size_t columnDimension) const { + if constexpr (!internal::HasQcqpVariableTraits::value) { + (void)costs; + (void)constraints; + (void)columnDimension; + throw std::runtime_error( + "FrobeniusBetweenFactor::qcqpFactors requires QCQP variable traits " + "for this type and matrix-form column dimensions (>= 2)."); + } else { + if (columnDimension < static_cast(N)) { + throw std::invalid_argument( + "FrobeniusBetweenFactor::qcqpFactors: columnDimension must be " + ">= the variable's intrinsic row dimension (e.g. >= 3 for Rot3)."); + } + if (!costs) { + throw std::invalid_argument( + "FrobeniusBetweenFactor::qcqpFactors costs is null"); + } + if (std::dynamic_pointer_cast(this->noiseModel_) || + this->noiseModel_->isConstrained()) { + throw std::runtime_error( + "FrobeniusBetweenFactor::qcqpFactors requires a " + "non-robust quadratic noise model"); + } + const auto isotropic = + std::dynamic_pointer_cast(this->noiseModel_); + if (!isotropic) { + throw std::runtime_error( + "FrobeniusBetweenFactor::qcqpFactors with column dimension > 1 " + "requires an isotropic noise model"); + } + + InsertQcqpConstraints(this->key1(), constraints); + InsertQcqpConstraints(this->key2(), constraints); + + const Matrix measurement = this->T12_.matrix(); + const Matrix I = Matrix::Identity(N, N); + const double weight = 1.0 / (isotropic->sigma() * isotropic->sigma()); + + Matrix B = Matrix::Zero(N, 2 * N); + B.block(0, 0, N, N) = -measurement.transpose(); + B.block(0, N, N, N) = I; + + const Matrix Q = weight * B.transpose() * B; + const SymmetricBlockMatrix blockQ(std::vector{N, N}, Q); + costs->push_back(std::make_shared( + KeyVector{this->key1(), this->key2()}, blockQ, columnDimension)); + } + } }; } // namespace gtsam