Skip to content
Merged
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
5 changes: 3 additions & 2 deletions gtsam/constrained/ActiveSetSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ namespace {
/* ************************************************************************* */
const Ordering& cachedOrdering(const GaussianFactorGraph& graph, KeySet* keys,
Ordering* ordering,
const std::optional<Ordering>& requested = {}) {
const Ordering* requested = nullptr) {
const KeySet graphKeys = graph.keys();
if (graphKeys != *keys) {
*ordering = requested ? restrictOrdering(*requested, graphKeys)
Expand Down Expand Up @@ -154,7 +154,8 @@ VectorValues ActiveSetSolver::solveSparseSubproblem(
const GaussianFactorGraph graph =
buildLpWorkingGraph(state.values, state.activeInequalityRows);
const Ordering& ordering = cachedOrdering(
graph, &workspace->lpKeys, &workspace->lpOrdering, params_->ordering);
graph, &workspace->lpKeys, &workspace->lpOrdering,
params_->ordering ? &*params_->ordering : nullptr);
return graph.optimize(ordering, EliminatePreferCholesky);
}
// Retain factor pointers and per-call options; the numerical backend
Expand Down
28 changes: 17 additions & 11 deletions gtsam/constrained/internal/SparseQpSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <gtsam/constrained/internal/SparseQpSolver.h>
#include <gtsam/linear/GaussianISAM2.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/linearAlgorithms-inst.h>

#include <algorithm>
#include <cmath>
Expand Down Expand Up @@ -139,20 +140,25 @@ struct SparseQpSolver::Impl {
return gradient;
}

// One iterative-refinement step removes roundoff amplified by weak working
// directions. For the constrained inverse K, x(delta - grad f(x)) equals
// x(delta) - K grad f(x); every solve retains the same affine hard rows.
// Only RHSs change: neither the factors nor their elimination are repeated.
// Refine with homogeneous tangent corrections, not another affine solve.
// Adding corrections to stored d before back-substitution can round them away
// or reintroduce its original error along weak directions. Hard rows have
// zero sigmas, so the cached inverse gives corrections tangent to their face.
VectorValues solveRefined(const GaussianISAM2& factorization,
const VectorValues& center,
VectorValues forcing) const {
const VectorValues candidate = forcing.size() == 0
const VectorValues& forcing) const {
VectorValues candidate = forcing.size() == 0
? factorization.optimize() : factorization.optimize(forcing);
for (const auto& [key, gradient] : workingGradient(candidate, center)) {
if (forcing.exists(key)) forcing.at(key) -= gradient;
else forcing.insert(key, -gradient);
for (size_t iteration = 0; iteration < 4; ++iteration) {
const VectorValues rhs = linearAlgorithms::transposeBayesTree(
factorization, workingGradient(candidate, center));
const VectorValues correction =
linearAlgorithms::optimizeBayesTree(factorization, &rhs);
VectorValues refined = candidate - correction;
if (refined.equals(candidate, 0.0)) break;
candidate = std::move(refined);
}
return factorization.optimize(forcing);
return candidate;
}

// A new center changes objective forcing only. Registered factors and orphan
Expand All @@ -169,7 +175,7 @@ struct SparseQpSolver::Impl {
if (!change.isZero(0.0)) forcing.insert(key, std::move(change));
}
}
return solveRefined(*tree, center, std::move(forcing));
return solveRefined(*tree, center, forcing);
}
};

Expand Down
10 changes: 10 additions & 0 deletions gtsam/constrained/tests/testLpSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,16 @@ TEST(ActiveSetSolver, Bounded1DLP) {
EXPECT_DOUBLES_EQUAL(2.0, result.at<Vector>(x)(0), 1e-7);
}

// An explicitly supplied ordering follows the cached LP solve path.
TEST(ActiveSetSolver, Bounded1DLPWithRequestedOrdering) {
auto params = std::make_shared<ActiveSetSolverParams>();
params->ordering = Ordering{x};
const Values result =
ActiveSetSolver(CreateBounded1D(), params).optimize(Values1(0.0));

EXPECT_DOUBLES_EQUAL(2.0, result.at<Vector>(x)(0), 1e-7);
}

// Verifies vector-valued no-initial LP solve uses phase-I initialization.
TEST(ActiveSetSolver, VectorOnlyNoInitialLP) {
const Values result = CreateBounded1D().optimize();
Expand Down
18 changes: 18 additions & 0 deletions gtsam/constrained/tests/testQpSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -846,6 +846,24 @@ TEST(ActiveSetSolver, SquareRootWeakDirection) {
CHECK(assert_equal(Vector2{1.0, 0.0}, result.first.at<Vector>(0), 1e-7));
}

// Homogeneous residual corrections must not reintroduce the rounded affine RHS.
TEST(ActiveSetSolver, SquareRootAffineRhsRefinement) {
const Matrix jacobian{{100000030, 30000009}, {1.0, 0.2}};
QpProblem problem;
problem.addCost(JacobianFactor(0, jacobian, Vector(jacobian.col(0))));
problem.addConstraint(LinearConstraint::LessEqual(
JacobianFactor(0, Matrix{{-1.0, 0.0}}, Vector1(0.0))));
auto params = std::make_shared<ActiveSetSolverParams>();
params->qpFormulation = ActiveSetSolverParams::QpFormulation::Primal;
params->regularization = 0;
params->maxIterations = 10;
Values initial;
initial.insert(0, Vector(Vector2{0.5, 0.0}));
const auto result = ActiveSetSolver(problem, params).optimizeWithState(initial);
CHECK(result.second.converged);
CHECK(assert_equal(Vector2{1.0, 0.0}, result.first.at<Vector>(0), 1e-7));
}

// A feasible exact subproblem minimum needs no duplicate factorization.
TEST(ActiveSetSolver, InspectUnregularizedMinimumImmediately) {
QpProblem problem;
Expand Down
14 changes: 13 additions & 1 deletion gtsam/constrained/tests/testSparseQpSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
#include <gtsam/linear/NoiseModel.h>
#include <CppUnitLite/TestHarness.h>

#include <limits>

using namespace gtsam;
using internal::SparseQpData;
using internal::SparseQpSolver;
Expand Down Expand Up @@ -105,7 +107,17 @@ TEST(SparseQpSolver, WeakAffineFaceResidualAccuracy) {
const double scale = direction.dot(target - (1 + weight) * rhs) /
(weight + (1 + weight) * direction.squaredNorm() * normal.squaredNorm());
const Vector error = result.at(1) - normal * scale;
CHECK(error.lpNorm<Eigen::Infinity>() < 5e-9);
// The two null directions have curvature weight, so absolute solution
// error is amplified by the reduced system's condition number. Check
// forward error at that scale, and stationarity independently below.
const double condition = 1 + (1 + weight) *
direction.squaredNorm() * normal.squaredNorm() / weight;
const double tolerance = std::numeric_limits<double>::epsilon() *
condition * (normal * scale).lpNorm<Eigen::Infinity>();
CHECK(error.lpNorm<Eigen::Infinity>() < tolerance);
const Vector gradient = weight * result.at(1) +
coupling.transpose() * ((1 + weight) * result.at(0) - target);
CHECK(gradient.lpNorm<Eigen::Infinity>() < 1e-12);
EXPECT(assert_equal(rhs, Vector(result.at(0) - coupling * result.at(1)), 1e-12));
}
}
Expand Down
6 changes: 5 additions & 1 deletion gtsam_unstable/linear/QPSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,11 @@ std::pair<VectorValues, VectorValues> QPSolver::optimize(
const QpProblem problem = ToConstrainedProblem(qp_);
const auto keyDims = CollectKeyDims(qp_);
const Values initial = ToValues(initialValues);
ActiveSetSolver solver(problem);
// The legacy overload requires a feasible initial point. Automatic may select
// Dual, which deliberately accepts infeasible starts, so retain Primal here.
auto params = std::make_shared<ActiveSetSolverParams>();
params->qpFormulation = ActiveSetSolverParams::QpFormulation::Primal;
ActiveSetSolver solver(problem, params);

try {
Values values;
Expand Down
3 changes: 3 additions & 0 deletions gtsam_unstable/linear/tests/testQPSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -244,6 +244,7 @@ TEST(QPSolver, failedSubproblem) {
}

/* ************************************************************************* */
// Both legacy supplied-point overloads reject infeasible initial values.
TEST(QPSolver, infeasibleInitial) {
QP qp;
qp.cost.add(X(1), I_2x2, Vector::Zero(2));
Expand All @@ -259,6 +260,8 @@ TEST(QPSolver, infeasibleInitial) {
QPSolver solver(qp);
VectorValues solution;
CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues);
CHECK_EXCEPTION(solver.optimize(initialValues, VectorValues(), true),
InfeasibleInitialValues);
}

#endif // GTSAM_ALLOW_DEPRECATED_SINCE_V43
Expand Down
Loading