Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
81 changes: 65 additions & 16 deletions gtsam/certifiable/LiftedSDPProblem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -219,6 +219,14 @@ SymbolicFactorGraph BuildQpCostSymbolicFactorGraph(const QcqpProblem& problem) {
sfg.push_back(SymbolicFactor(*cost));
}

// A multi-key linear equality couples its keys like a cost does, so they
// must share a clique for its lifted rows to exist.
for (const auto& factor : problem.eConstraints()) {
if (factor && factor->size() > 1) {
sfg.push_back(SymbolicFactor(*factor));
}
}

if (sfg.empty()) {
throw std::runtime_error(
"BuildQpCostSymbolicFactorGraph: QCQP has no objective costs.");
Expand Down Expand Up @@ -479,30 +487,43 @@ void AddQuadraticConstraint(
}
}

// Lower Ax = b on one key to both rows of [-b A] [1 y'; y X] = 0.
// Lower sum_i A_i x_i = b over the keys of one linear equality to the rows of
// [-b A_1 ... A_m] [1 y'; y X] = 0, one block of rows per key.
void AddLinearEqualityConstraint(
const mf::Model::t& M, const LinearConstraint& equality,
const KeyiToLiftedVectorxiViewInSDPVariableMap& xiMap,
const KeysijToLiftedVariableXijViewInSDPVariableMap& xijMap) {
const JacobianFactor& J = equality.factor();
if (J.size() != 1) {
throw std::runtime_error(
"LiftedSDPProblem: only unary linear equality QCQP constraints are "
"supported.");
}
const Key key = *J.begin();
const auto y = xiMap.at(key)->asExpr();
const auto X = xijMap.at({key, key})->asExpr();
const auto A = convertToMosekDenseMatrix(Matrix(J.getA(J.begin())));
const auto b = convertToMosekDenseMatrix(Vector(J.getb()));
auto sumOverKeys = [&](auto&& term) {
auto it = J.begin();
mf::Expression::t sum =
term(convertToMosekDenseMatrix(Matrix(J.getA(it))), *it);
for (++it; it != J.end(); ++it) {
sum = mf::Expr::add(
sum, term(convertToMosekDenseMatrix(Matrix(J.getA(it))), *it));
}
return sum;
};

// Ax=b implies A*x*x'=b*x', hence A*X=b*y' after lifting. Enforcing only
// Ay=b on the lifted vector x_i (row-0 slice) would leave unconstrained PSD
// slack in X.
M->constraint(mf::Expr::mul(A, mf::Expr::transpose(y)),
// Sum_i A_i y_i = b on the lifted vectors x_i (row-0 slices).
M->constraint(sumOverKeys([&](const auto& A, Key key) {
return mf::Expr::mul(
A, mf::Expr::transpose(xiMap.at(key)->asExpr()));
}),
mf::Domain::equalsTo(b));
M->constraint(mf::Expr::sub(mf::Expr::mul(A, X), mf::Expr::mul(b, y)),
mf::Domain::equalsTo(0.0));

// The equality times x_j' gives sum_i A_i X_ij = b*y_j' after lifting.
// Enforcing only the row-0 equality would leave unconstrained PSD slack in X.
for (auto jt = J.begin(); jt != J.end(); ++jt) {
const Key keyJ = *jt;
const auto sumOverI = sumOverKeys([&](const auto& A, Key keyI) {
return mf::Expr::mul(A, xijMap.at({keyI, keyJ})->asExpr());
});
M->constraint(
mf::Expr::sub(sumOverI, mf::Expr::mul(b, xiMap.at(keyJ)->asExpr())),
mf::Domain::equalsTo(0.0));
}
}

// Add all QCQP equality and inequality constraints to a Fusion model.
Expand Down Expand Up @@ -636,6 +657,31 @@ QpCost::shared_ptr SubstituteFixedVariables(const QpCost& cost,
HessianFactor(freeKeys, SymmetricBlockMatrix(freeDims, Q)));
}

// A multi-key linear equality that involves a fixed variable keeps its free
// terms and moves the fixed terms A_K v to the right-hand side, so the
// coupling survives the substitution. Returns null for other constraints,
// which are dropped, and for equalities with no free variable left.
NonlinearEqualityConstraint::shared_ptr SubstituteFixedLinearEquality(
const NonlinearEqualityConstraint& factor, const Values& fixedVariables) {
const auto* linear = dynamic_cast<const LinearEqualityConstraintFactor*>(&factor);
if (!linear || linear->size() < 2) return nullptr;
const JacobianFactor& J = linear->linearConstraint().factor();
std::vector<std::pair<Key, Matrix>> freeTerms;
Vector b = J.getb();
for (auto it = J.begin(); it != J.end(); ++it) {
if (fixedVariables.exists(*it)) {
const Matrix& value = fixedVariables.at<Matrix>(*it);
b -= J.getA(it) * Eigen::Map<const Vector>(value.data(), value.size());
} else {
freeTerms.emplace_back(*it, Matrix(J.getA(it)));
}
}
if (freeTerms.empty()) return nullptr;
return LinearConstraint::Equal(JacobianFactor(freeTerms, b, J.get_model()),
linear->linearConstraint().sigmas())
.createEqualityFactor();
}

// Substitute the fixed variables out of a QCQP before it is lifted. Costs are
// rewritten on the free variables. The constraints that involve a fixed
// variable are dropped: from a factor graph these are its unary manifold
Expand Down Expand Up @@ -671,6 +717,9 @@ QcqpProblem SubstituteFixedVariables(const QcqpProblem& problem,
for (const auto& factor : problem.eConstraints()) {
if (!InvolvesFixedVariable(factor->keys(), *fixedVariables)) {
eqConstraints.push_back(factor);
} else if (auto substituted =
SubstituteFixedLinearEquality(*factor, *fixedVariables)) {
eqConstraints.push_back(substituted);
}
}
NonlinearInequalityConstraints ineqConstraints;
Expand Down
20 changes: 16 additions & 4 deletions gtsam/certifiable/LiftedSDPProblem.h
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,14 @@ class GTSAM_EXPORT LiftedSDPProblem<MonolithicSDP, MosekSDPSolver> {
* Fixed variables are substituted out before lifting: a unary linear
* equality I x = v, which a hard FrobeniusPrior emits, fixes x to v. The
* costs that involve it are rewritten on the remaining variables, the
* constraints that involve it are dropped, and it gets no SDP block, so no
* singular rank-one block is pinned. The QCQP itself is unchanged.
* quadratic constraints that involve it are dropped, linear equalities that
* couple it to free variables keep their free terms, and it gets no SDP
* block, so no singular rank-one block is pinned. The QCQP itself is
* unchanged.
*
* Linear equalities over several keys, A_1 x_1 + ... + A_n x_n = b, are
* relaxed with the products A x_j' - b y_j' = 0 for every key j, in addition
* to the lifted constant row, and join the chordal sparsity graph.
*
* @param problem QCQP to relax.
*/
Expand Down Expand Up @@ -147,8 +153,14 @@ class GTSAM_EXPORT LiftedSDPProblem<ChordalSDP, MosekSDPSolver> {
* Fixed variables are substituted out before lifting: a unary linear
* equality I x = v, which a hard FrobeniusPrior emits, fixes x to v. The
* costs that involve it are rewritten on the remaining variables, the
* constraints that involve it are dropped, and it gets no SDP block, so no
* singular rank-one block is pinned. The QCQP itself is unchanged.
* quadratic constraints that involve it are dropped, linear equalities that
* couple it to free variables keep their free terms, and it gets no SDP
* block, so no singular rank-one block is pinned. The QCQP itself is
* unchanged.
*
* Linear equalities over several keys, A_1 x_1 + ... + A_n x_n = b, are
* relaxed with the products A x_j' - b y_j' = 0 for every key j, in addition
* to the lifted constant row, and join the chordal sparsity graph.
*
* @param problem QCQP to relax.
* @param orderingType Ordering used for symbolic elimination.
Expand Down
98 changes: 98 additions & 0 deletions gtsam/certifiable/tests/testLiftedSDPs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -702,6 +702,104 @@ TEST(LiftedSDPs, QuadraticBoundaryEquality) {

} // namespace natural_form_fixture
/* ************************************************************************* */
namespace multi_key_equality_fixture {

const Vector2 p(1.0, 0.0), q(0.0, 1.0), c(2.0, 3.0);

// Two unary costs 1/2||x0-p||^2 + 1/2||x1-q||^2 whose keys are coupled only by
// the linear equality x0 + x1 = c, so the equality alone connects the keys.
QcqpProblem CoupledProblem() {
NonlinearFactorGraph costs;
costs.emplace_shared<QpCost>(HessianFactor(
0, Matrix(Matrix2::Identity()), Vector(p), p.squaredNorm()));
costs.emplace_shared<QpCost>(HessianFactor(
1, Matrix(Matrix2::Identity()), Vector(q), q.squaredNorm()));
QcqpProblem problem(costs, NonlinearEqualityConstraints());
problem.addConstraint(LinearConstraint::Equal(
JacobianFactor(0, Matrix2::Identity(), 1, Matrix2::Identity(), c)));
return problem;
}

// Runs the monolithic and both chordal formulations on a problem.
template <typename Check>
void CheckAllFormulations(const QcqpProblem& problem, const Check& check) {
MosekMonolithicSDP monolithic(problem);
check(&monolithic);
for (const auto ordering :
{ChordalOrderingType::Colamd, ChordalOrderingType::Metis}) {
MosekChordalSDP chordal(problem, ordering);
check(&chordal);
}
}

// A linear equality over two keys is lowered to sum_i A_i X_ij = b y_j' for
// each key j, and its keys share a chordal clique. The optimum shifts both
// points by (c - p - q) / 2, so the objective is |c - p - q|^2 / 4.
TEST(LiftedSDPs, MultiKeyLinearEquality) {
const QcqpProblem problem = CoupledProblem();
const Vector2 shift = (c - p - q) / 2.0;
const double expected = 0.25 * (c - p - q).squaredNorm();
CheckAllFormulations(problem, [&](auto* solver) {
EXPECT(solver->solve());
EXPECT_DOUBLES_EQUAL(expected, solver->objectiveValue(), 1e-6);
const Values values = solver->qcqpValues();
EXPECT(assert_equal(Vector2(p + shift), Vector(values.at<Matrix>(0).col(0)),
1e-5));
EXPECT(assert_equal(Vector2(q + shift), Vector(values.at<Matrix>(1).col(0)),
1e-5));
EXPECT_DOUBLES_EQUAL(expected, problem.costs().error(values), 1e-5);
EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(values),
1e-5);
for (double ratio : solver->variableEVRs()) EXPECT(ratio > 1e4);
});
}

// A multi-key equality with a fixed variable keeps its free terms and moves the
// fixed term to the right-hand side: x0 = v gives x1 = c - v.
TEST(LiftedSDPs, MultiKeyLinearEqualityWithFixedVariable) {
const Vector2 v(0.5, -1.0);
QcqpProblem problem = CoupledProblem();
problem.addConstraint(
LinearConstraint::Equal(JacobianFactor(0, Matrix2::Identity(), v)));
const double expected =
0.5 * (v - p).squaredNorm() + 0.5 * (c - v - q).squaredNorm();
CheckAllFormulations(problem, [&](auto* solver) {
EXPECT(solver->solve());
EXPECT_DOUBLES_EQUAL(expected, solver->objectiveValue(), 1e-6);
const Values values = solver->qcqpValues();
EXPECT(assert_equal(v, Vector(values.at<Matrix>(0).col(0)), 1e-5));
EXPECT(assert_equal(Vector2(c - v), Vector(values.at<Matrix>(1).col(0)),
1e-5));
});
}

// Keys of different sizes read rectangular off-diagonal blocks: with
// [1 0] x0 - [0 1 0] x1 = 0, the first key's leading entry is tied to the
// second key's middle entry.
TEST(LiftedSDPs, MultiKeyLinearEqualityRectangularBlocks) {
const Vector2 target0(1.0, 0.0);
const Vector3 target1(0.0, 3.0, 0.0);
NonlinearFactorGraph costs;
costs.emplace_shared<QpCost>(HessianFactor(
0, Matrix(Matrix2::Identity()), Vector(target0), target0.squaredNorm()));
costs.emplace_shared<QpCost>(HessianFactor(
1, Matrix(Matrix3::Identity()), Vector(target1), target1.squaredNorm()));
QcqpProblem problem(costs, NonlinearEqualityConstraints());
problem.addConstraint(LinearConstraint::Equal(JacobianFactor(
0, Matrix(Vector2(1.0, 0.0).transpose()), 1,
Matrix(-Vector3(0.0, 1.0, 0.0).transpose()), Vector1::Zero())));
// The tied entries meet halfway: t = (1 + 3) / 2 = 2, costing 1/2 + 1/2.
CheckAllFormulations(problem, [&](auto* solver) {
EXPECT(solver->solve());
EXPECT_DOUBLES_EQUAL(1.0, solver->objectiveValue(), 1e-6);
const Values values = solver->qcqpValues();
EXPECT_DOUBLES_EQUAL(2.0, values.at<Matrix>(0)(0), 1e-5);
EXPECT_DOUBLES_EQUAL(2.0, values.at<Matrix>(1)(1), 1e-5);
});
}

} // namespace multi_key_equality_fixture
/* ************************************************************************* */
namespace fixed_variable_fixture {

constexpr size_t kCount = 4;
Expand Down
89 changes: 88 additions & 1 deletion gtsam/nonlinear/WnoaFactor.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,18 +22,44 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/constrained/QcqpProblem.h>
#include <gtsam/geometry/Point1.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/WnoaStateData.h>

#include <cassert>
#include <stdexcept>
#include <type_traits>

namespace gtsam {

namespace internal {

/// Row of a type's D=1 QCQP vector where its Euclidean coordinates start.
/// Defaults to 0; types with extra lifted rows define QcqpCoordinateOffset.
template <typename T, typename = void>
struct QcqpCoordinateOffset : std::integral_constant<int, 0> {};

template <typename T>
struct QcqpCoordinateOffset<
T, std::void_t<decltype(traits<T>::QcqpCoordinateOffset)>>
: std::integral_constant<int, traits<T>::QcqpCoordinateOffset> {};

/// True for vector-space poses whose D=1 QCQP vector holds their coordinates.
template <typename Pose>
inline constexpr bool kWnoaQcqpSupported =
std::is_same_v<typename traits<Pose>::structure_category,
vector_space_tag> &&
HasQcqpVariableTraits<Pose, 1>::value &&
HasQcqpVectorDim<Pose>::value &&
HasQcqpVariableTraits<typename traits<Pose>::TangentVector, 1>::value;

} // namespace internal

/**
* @brief WNOA (White Noise on Acceleration) motion prior factor.
*
Expand Down Expand Up @@ -199,6 +225,67 @@ class WnoaMotionFactor
}
/// @}

/**
* Append this factor's exact D=1 QCQP cost.
*
* For vector-space poses the residual is linear in the state:
* [p2 - p1 - dt*v1; v2 - v1]. With lifted pose vectors X1 and X2 whose
* Euclidean coordinates start at row s, it equals B*[X1; v1; X2; v2] with
* B = [[-S, -dt*I, S, 0], [0, -I, 0, I]] and S = [0 I 0] selecting rows
* s..s+N-1. The whitened cost is 0.5*||W*B*X||^2 and needs no constraints
* beyond those of the keys' own lifts. The Hessian's final constant slot
* has zero coefficient.
*
* Only vector-space poses with QCQP traits (for example AugmentedPoint)
* are supported.
*
* @throws std::invalid_argument for columnDimension other than 1.
* @throws std::runtime_error for unsupported pose types.
*/
void qcqpFactors(NonlinearFactorGraph& costs,
NonlinearEqualityConstraints& constraints,
QcqpVariableConstraints& variableConstraints,
size_t columnDimension = 1) const override {
if constexpr (!internal::kWnoaQcqpSupported<Pose>) {
throw std::runtime_error(
"WnoaMotionFactor::qcqpFactors requires a vector-space pose with "
"QCQP traits, such as AugmentedPoint.");
} else {
if (columnDimension != 1) {
throw std::invalid_argument(
"WnoaMotionFactor::qcqpFactors only supports D=1.");
}
constexpr int poseDim = traits<Pose>::QcqpVectorDim;
constexpr int offset = internal::QcqpCoordinateOffset<Pose>::value;
constexpr int liftedDim = 2 * (poseDim + dim);
// Columns: [X1 (poseDim) | v1 | X2 (poseDim) | v2 | constant slot].
constexpr int pose1 = 0, vel1 = poseDim, pose2 = poseDim + dim,
vel2 = 2 * poseDim + dim;
Matrix B = Matrix::Zero(2 * dim, liftedDim + 1);
B.block<dim, dim>(0, pose1 + offset) = -kIdentity;
B.block<dim, dim>(0, vel1) = -deltaT_ * kIdentity;
B.block<dim, dim>(0, pose2 + offset) = kIdentity;
B.block<dim, dim>(dim, vel1) = -kIdentity;
B.block<dim, dim>(dim, vel2) = kIdentity;

const Matrix whitenedB = this->noiseModel_->Whiten(B);
const KeyVector keys{this->key1(), this->key2(), this->key3(),
this->key4()};
costs.push_back(std::make_shared<QpCost>(HessianFactor(
keys, SymmetricBlockMatrix(
std::vector<DenseIndex>{poseDim, dim, poseDim, dim, 1},
whitenedB.transpose() * whitenedB))));
variableConstraints.add(this->key1(),
traits<Pose>::template QcqpConstraints<1>);
variableConstraints.add(this->key2(),
traits<Velocity>::template QcqpConstraints<1>);
variableConstraints.add(this->key3(),
traits<Pose>::template QcqpConstraints<1>);
variableConstraints.add(this->key4(),
traits<Velocity>::template QcqpConstraints<1>);
}
}

/**
* @brief Evaluate the WNOA factor residual and optional Jacobians.
*
Expand Down
Loading