12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactor QPSolver to work with single-valued linear inequality factors. Unit tests passed.

release/4.3a0
thduynguyen 2014-12-12 12:03:00 -05:00
parent 6d68f5ffff
commit a26ef7b7a2
3 changed files with 117 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
gtsam_unstable/linear/QPSolver.cpp
View File

@ -31,7 +31,10 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) {
VectorValues QPSolver::solveWithCurrentWorkingSet( VectorValues QPSolver::solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const { const LinearInequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; GaussianFactorGraph workingGraph = baseGraph_;
workingGraph.push_back(workingSet); BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
if (factor->active())
workingGraph.push_back(factor);
}
return workingGraph.optimize(); return workingGraph.optimize();
} }
@ -48,30 +51,19 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
AtermsInequalities.end()); AtermsInequalities.end());
// Collect the gradients of unconstrained cost factors to the b vector // Collect the gradients of unconstrained cost factors to the b vector
Vector b = zero(delta.at(key).size()); if (Aterms.size() > 0) {
if (costVariableIndex_.find(key) != costVariableIndex_.end()) { Vector b = zero(delta.at(key).size());
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
b += factor->gradient(key, delta); GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
b += factor->gradient(key, delta);
}
} }
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows()));
} }
else {
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows())); return boost::make_shared<JacobianFactor>();
}
//******************************************************************************
GaussianFactor::shared_ptr QPSolver::createDualPrior(
const LinearInequality::shared_ptr& factor) const {
Vector sigmas = factor->get_model()->sigmas();
size_t n = sigmas.rows();
Matrix A = eye(n);
for (size_t i = 0; i<n; ++i) {
if (sigmas[i] == ACTIVE)
A(i,i) = 0;
} }
Vector b = zero(n);
return boost::make_shared<JacobianFactor>(factor->dualKey(), A, b);
} }
//****************************************************************************** //******************************************************************************
@ -80,50 +72,32 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
BOOST_FOREACH(Key key, constrainedKeys_) { BOOST_FOREACH(Key key, constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph // Each constrained key becomes a factor in the dual graph
dualGraph->push_back(createDualFactor(key, workingSet, delta)); JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta);
} if (!dualFactor->empty())
dualGraph->push_back(dualFactor);
// Add prior for inactive dual variables
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
dualGraph->push_back(createDualPrior(factor));
} }
return dualGraph; return dualGraph;
} }
//****************************************************************************** //******************************************************************************
pair<int, int> QPSolver::identifyLeavingConstraint( int QPSolver::identifyLeavingConstraint(
const LinearInequalityFactorGraph& workingSet, const LinearInequalityFactorGraph& workingSet,
const VectorValues& lambdas) const { const VectorValues& lambdas) const {
int worstFactorIx = -1, worstSigmaIx = -1; int worstFactorIx = -1;
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
// inactive or a good inequality constraint, so we don't care! // inactive or a good inequality constraint, so we don't care!
double maxLambda = 0.0; double maxLambda = 0.0;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
Vector lambda = lambdas.at(factor->dualKey()); if (factor->active()) {
Vector sigmas = factor->get_model()->sigmas(); double lambda = lambdas.at(factor->dualKey())[0];
for (size_t j = 0; j < sigmas.size(); ++j) if (lambda > maxLambda) {
// If it is an active constraint, and lambda is larger than the current max
if (sigmas[j] == ACTIVE && lambda[j] > maxLambda) {
worstFactorIx = factorIx; worstFactorIx = factorIx;
worstSigmaIx = j; maxLambda = lambda;
maxLambda = lambda[j];
} }
}
} }
return make_pair(worstFactorIx, worstSigmaIx); return worstFactorIx;
}
//******************************************************************************
LinearInequalityFactorGraph QPSolver::updateWorkingSet(
const LinearInequalityFactorGraph& workingSet, int factorIx, int sigmaIx,
double state) const {
LinearInequalityFactorGraph newWorkingSet = workingSet;
if (factorIx < 0 || sigmaIx < 0)
return newWorkingSet;
Vector sigmas = newWorkingSet.at(factorIx)->get_model()->sigmas();
sigmas[sigmaIx] = state;
newWorkingSet.at(factorIx)->setModel(true, sigmas);
return newWorkingSet;
} }
//****************************************************************************** //******************************************************************************
@ -142,45 +116,43 @@ LinearInequalityFactorGraph QPSolver::updateWorkingSet(
* *
* We want the minimum of all those alphas among all inactive inequality. * We want the minimum of all those alphas among all inactive inequality.
*/ */
boost::tuple<double, int, int> QPSolver::computeStepSize( boost::tuple<double, int> QPSolver::computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const { const VectorValues& p) const {
static bool debug = false; static bool debug = false;
double minAlpha = 1.0; double minAlpha = 1.0;
int closestFactorIx = -1, closestSigmaIx = -1; int closestFactorIx = -1;
for(size_t factorIx = 0; factorIx<workingSet.size(); ++factorIx) { for(size_t factorIx = 0; factorIx<workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
Vector sigmas = factor->get_model()->sigmas(); double b = factor->getb()[0];
Vector b = factor->getb(); // only check inactive factors
for (size_t s = 0; s < sigmas.size(); ++s) { if (!factor->active()) {
// If it is an inactive inequality, compute alpha and update min // Compute a'*p
if (sigmas[s] == INACTIVE) { double aTp = factor->dotProductRow(p);
// Compute aj'*p
double ajTp = factor->dotProductRow(s, p);
// Check if aj'*p >0. Don't care if it's not. // Check if a'*p >0. Don't care if it's not.
if (ajTp <= 0) if (aTp <= 0)
continue; continue;
// Compute aj'*xk // Compute a'*xk
double ajTx = factor->dotProductRow(s, xk); double aTx = factor->dotProductRow(xk);
// alpha = (bj - aj'*xk) / (aj'*p) // alpha = (b - a'*xk) / (a'*p)
double alpha = (b[s] - ajTx) / ajTp; double alpha = (b - aTx) / aTp;
if (debug) if (debug)
cout << "alpha: " << alpha << endl; cout << "alpha: " << alpha << endl;
// We want the minimum of all those max alphas // We want the minimum of all those max alphas
if (alpha < minAlpha) { if (alpha < minAlpha) {
closestFactorIx = factorIx; closestFactorIx = factorIx;
closestSigmaIx = s; minAlpha = alpha;
minAlpha = alpha;
}
} }
} }
} }
return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx);
return boost::make_tuple(minAlpha, closestFactorIx);
} }
//****************************************************************************** //******************************************************************************
@ -204,21 +176,18 @@ QPState QPSolver::iterate(const QPState& state) const {
if (debug) if (debug)
duals.print("Duals :"); duals.print("Duals :");
int leavingFactor, leavingSigmaIx; int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
boost::tie(leavingFactor, leavingSigmaIx) = //
identifyLeavingConstraint(state.workingSet, duals);
if (debug) if (debug)
cout << "violated active inequality - factorIx, sigmaIx: " << leavingFactor cout << "leavingFactor: " << leavingFactor << endl;
<< " " << leavingSigmaIx << endl;
// If all inequality constraints are satisfied: We have the solution!! // If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0 || leavingSigmaIx < 0) { if (leavingFactor < 0) {
return QPState(newValues, duals, state.workingSet, true); return QPState(newValues, duals, state.workingSet, true);
} }
else { else {
// Inactivate the leaving constraint // Inactivate the leaving constraint
LinearInequalityFactorGraph newWorkingSet = updateWorkingSet( LinearInequalityFactorGraph newWorkingSet = state.workingSet;
state.workingSet, leavingFactor, leavingSigmaIx, INACTIVE); newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false); return QPState(newValues, duals, newWorkingSet, false);
} }
} }
@ -226,16 +195,18 @@ QPState QPSolver::iterate(const QPState& state) const {
// If we CAN make some progress // If we CAN make some progress
// Adapt stepsize if some inactive constraints complain about this move // Adapt stepsize if some inactive constraints complain about this move
double alpha; double alpha;
int factorIx, sigmaIx; int factorIx;
VectorValues p = newValues - state.values; VectorValues p = newValues - state.values;
boost::tie(alpha, factorIx, sigmaIx) = // boost::tie(alpha, factorIx) = //
computeStepSize(state.workingSet, state.values, p); computeStepSize(state.workingSet, state.values, p);
if (debug) if (debug)
cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " cout << "alpha, factorIx: " << alpha << " " << factorIx << " "
<< sigmaIx << endl; << endl;
// also add to the working set the one that complains the most // also add to the working set the one that complains the most
LinearInequalityFactorGraph newWorkingSet = // LinearInequalityFactorGraph newWorkingSet = state.workingSet;
updateWorkingSet(state.workingSet, factorIx, sigmaIx, ACTIVE); if (factorIx >= 0)
newWorkingSet.at(factorIx)->activate();
// step! // step!
newValues = state.values + alpha * p; newValues = state.values + alpha * p;
@ -251,14 +222,12 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
LinearInequalityFactorGraph workingSet; LinearInequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
Vector e = workingFactor->error_vector(initialValues); double error = workingFactor->error(initialValues);
Vector sigmas = zero(e.rows()); if (fabs(error)>1e-7){
for (int i = 0; i < e.rows(); ++i){ workingFactor->inactivate();
if (fabs(e[i])>1e-7){ } else {
sigmas[i] = INACTIVE; workingFactor->activate();
}
} }
workingFactor->setModel(true, sigmas);
workingSet.push_back(workingFactor); workingSet.push_back(workingFactor);
} }
return workingSet; return workingSet;
@ -410,13 +379,12 @@ boost::tuple<LinearEqualityFactorGraph::shared_ptr,
// Add the slack term to the constraint // Add the slack term to the constraint
// Unlike Nocedal06book, pg.473, we want ax-z <= b, since we always assume // Unlike Nocedal06book, pg.473, we want ax-z <= b, since we always assume
// LE constraints ax <= b. // LE constraints ax <= b.
size_t dim = factor->rows(); terms.push_back(make_pair(slackKey, -eye(1)));
terms.push_back(make_pair(slackKey, -eye(dim))); inequalities->push_back(LinearInequality(terms, factor->getb()[0],
inequalities->push_back(LinearInequality(terms, factor->getb(),
factor->dualKey())); factor->dualKey()));
// Add lower bound for this slack key // Add lower bound for this slack key
slackLowerBounds.insert(slackKey, zero(dim)); slackLowerBounds.insert(slackKey, zero(1));
// Increase slackKey for the next slack variable // Increase slackKey for the next slack variable
slackKey++; slackKey++;
} }

24
gtsam_unstable/linear/QPSolver.h
View File

@ -15,9 +15,6 @@
namespace gtsam { namespace gtsam {
static const double ACTIVE = 0.0;
static const double INACTIVE = std::numeric_limits<double>::infinity();
/// This struct holds the state of QPSolver at each iteration /// This struct holds the state of QPSolver at each iteration
struct QPState { struct QPState {
VectorValues values; VectorValues values;
@ -73,6 +70,7 @@ public:
if (variableIndex.find(key) != variableIndex.end()) { if (variableIndex.find(key) != variableIndex.end()) {
BOOST_FOREACH(size_t factorIx, variableIndex[key]){ BOOST_FOREACH(size_t factorIx, variableIndex[key]){
typename FACTOR::shared_ptr factor = graph.at(factorIx); typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active()) continue;
Matrix Ai = factor->getA(factor->find(key)).transpose(); Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
} }
@ -85,15 +83,6 @@ public:
const LinearInequalityFactorGraph& workingSet, const LinearInequalityFactorGraph& workingSet,
const VectorValues& delta) const; const VectorValues& delta) const;
/**
* Create dummy prior for inactive dual variables
* TODO: This might be inefficient but I don't know how to avoid
* the Indeterminant exception due to no information for the duals
* on inactive components of the constraints.
*/
GaussianFactor::shared_ptr createDualPrior(
const LinearInequality::shared_ptr& factor) const;
/** /**
* Build the dual graph to solve for the Lagrange multipliers. * Build the dual graph to solve for the Lagrange multipliers.
* *
@ -161,17 +150,10 @@ public:
* And we want to remove the worst one with the largest lambda from the active set. * And we want to remove the worst one with the largest lambda from the active set.
* *
*/ */
std::pair<int, int> identifyLeavingConstraint( int identifyLeavingConstraint(
const LinearInequalityFactorGraph& workingSet, const LinearInequalityFactorGraph& workingSet,
const VectorValues& lambdas) const; const VectorValues& lambdas) const;
/**
* Inactivate or activate an inequality constraint
*/
LinearInequalityFactorGraph updateWorkingSet(
const LinearInequalityFactorGraph& workingSet, int factorIx, int sigmaIx,
double state) const;
/** /**
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
* *
@ -180,7 +162,7 @@ public:
* This constraint will be added to the working set and become active * This constraint will be added to the working set and become active
* in the next iteration * in the next iteration
*/ */
boost::tuple<double, int, int> computeStepSize( boost::tuple<double, int> computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const; const VectorValues& p) const;

104
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -26,6 +26,8 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand; using namespace gtsam::symbol_shorthand;
const Matrix One = ones(1,1);
/* ************************************************************************* */ /* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5 // Create test graph according to Forst10book_pg171Ex5
QP createTestCase() { QP createTestCase() {
@ -40,12 +42,10 @@ QP createTestCase() {
2.0 * ones(1, 1), zero(1), 10.0)); 2.0 * ones(1, 1), zero(1), 10.0));
// Inequality constraints // Inequality constraints
// Jacobian factors represent Ax-b, hence qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
// x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
Matrix A1 = (Matrix(4, 1) << 1, -1, 0, 1); qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
Matrix A2 = (Matrix(4, 1) << 1, 0, -1, 0); qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2
Vector b = (Vector(4) << 2, 0, 0, 1.5);
qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0));
return qp; return qp;
} }
@ -66,20 +66,20 @@ TEST(QPSolver, constraintsAux) {
QPSolver solver(qp); QPSolver solver(qp);
VectorValues lambdas; VectorValues lambdas;
lambdas.insert(0, (Vector(4) << -0.5, 0.0, 0.3, 0.1)); lambdas.insert(0, (Vector(1) << -0.5));
int factorIx, lambdaIx; lambdas.insert(1, (Vector(1) << 0.0));
boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint( lambdas.insert(2, (Vector(1) << 0.3));
qp.inequalities, lambdas); lambdas.insert(3, (Vector(1) << 0.1));
LONGS_EQUAL(0, factorIx); int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas);
LONGS_EQUAL(2, lambdaIx); LONGS_EQUAL(2, factorIx);
VectorValues lambdas2; VectorValues lambdas2;
lambdas2.insert(0, (Vector(4) << -0.5, 0.0, -0.3, -0.1)); lambdas2.insert(0, (Vector(1) << -0.5));
int factorIx2, lambdaIx2; lambdas2.insert(1, (Vector(1) << 0.0));
boost::tie(factorIx2, lambdaIx2) = solver.identifyLeavingConstraint( lambdas2.insert(2, (Vector(1) << -0.3));
qp.inequalities, lambdas2); lambdas2.insert(3, (Vector(1) << -0.1));
int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2);
LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, factorIx2);
LONGS_EQUAL(-1, lambdaIx2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -135,9 +135,10 @@ TEST(QPSolver, indentifyActiveConstraints) {
LinearInequalityFactorGraph workingSet = LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution); solver.identifyActiveConstraints(qp.inequalities, currentSolution);
Vector actualSigmas = workingSet.at(0)->get_model()->sigmas(); CHECK(!workingSet.at(0)->active()); // inactive
Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE); CHECK(workingSet.at(1)->active()); // active
CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100)); CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active()); // inactive
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
VectorValues expectedSolution; VectorValues expectedSolution;
@ -159,19 +160,18 @@ TEST(QPSolver, iterate) {
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4); std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0)); expectedSolutions[0].insert(X(1), (Vector(1) << 0.0));
expectedSolutions[0].insert(X(2), (Vector(1) << 0.0)); expectedSolutions[0].insert(X(2), (Vector(1) << 0.0));
expectedDuals[0].insert(0, (Vector(4) << 0, 3, 0, 0)); expectedDuals[0].insert(1, (Vector(1) << 3));
expectedDuals[0].insert(2, (Vector(1) << 0));
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5));
expectedSolutions[1].insert(X(2), (Vector(1) << 0.0)); expectedSolutions[1].insert(X(2), (Vector(1) << 0.0));
expectedDuals[1].insert(0, (Vector(4) << 0, 0, 1.5, 0)); expectedDuals[1].insert(3, (Vector(1) << 1.5));
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5));
expectedSolutions[2].insert(X(2), (Vector(1) << 0.75)); expectedSolutions[2].insert(X(2), (Vector(1) << 0.75));
expectedDuals[2].insert(0, (Vector(4) << 0, 0, 1.5, 0));
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[3].insert(X(1), (Vector(1) << 1.5));
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5)); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5));
expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0));
LinearInequalityFactorGraph workingSet = LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution); solver.identifyActiveConstraints(qp.inequalities, currentSolution);
@ -223,12 +223,11 @@ QP createTestMatlabQPEx() {
2.0 * ones(1, 1), 6 * ones(1), 1000.0)); 2.0 * ones(1, 1), 6 * ones(1), 1000.0));
// Inequality constraints // Inequality constraints
// Jacobian factors represent Ax-b, hence qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2
// x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2
Matrix A1 = (Matrix(5, 1) << 1, -1, 2, -1, 0); qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3
Matrix A2 = (Matrix(5, 1) << 1, 2, 1, 0, -1); qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0
Vector b = (Vector(5) << 2, 2, 3, 0, 0); qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0));
return qp; return qp;
} }
@ -256,16 +255,11 @@ QP createTestNocedal06bookEx16_4() {
qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1)));
// Inequality constraints // Inequality constraints
qp.inequalities.push_back( qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1), qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1));
0)); qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2));
qp.inequalities.push_back( qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3));
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4));
qp.inequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1),
2));
qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3));
qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4));
return qp; return qp;
} }
@ -317,16 +311,11 @@ QP modifyNocedal06bookEx16_4() {
// Inequality constraints // Inequality constraints
noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::shared_ptr noise =
noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); noiseModel::Constrained::MixedSigmas((Vector(1) << -1));
qp.inequalities.push_back( qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, -1, 0));
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1), qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1));
0)); qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2));
qp.inequalities.push_back( qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3));
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4));
qp.inequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1),
2));
qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3));
qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4));
return qp; return qp;
} }
@ -360,18 +349,15 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
LinearInequalityFactorGraph expectedInequalities; LinearInequalityFactorGraph expectedInequalities;
expectedInequalities.push_back( expectedInequalities.push_back(
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3), LinearInequality(X(1), -One, X(2), 2 * One, X(3), -One, -1, 0));
-ones(1, 1), -1 * ones(1), 0));
expectedInequalities.push_back( expectedInequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4), LinearInequality(X(1), One, X(2), 2 * One, X(4), -One, 6, 1));
-ones(1, 1), 6 * ones(1), 1));
expectedInequalities.push_back( expectedInequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5), LinearInequality(X(1), One, X(2), -2 * One, X(5), -One, 2, 2));
-ones(1, 1), 2 * ones(1), 2));
expectedInequalities.push_back( expectedInequalities.push_back(
LinearInequality(X(1), -ones(1, 1), X(6), -ones(1, 1), zero(1), 3)); LinearInequality(X(1), -One, X(6), -One, 0, 3));
expectedInequalities.push_back( expectedInequalities.push_back(
LinearInequality(X(2), -ones(1, 1), X(7), -ones(1, 1), zero(1), 4)); LinearInequality(X(2), -One, X(7), -One, 0, 4));
EXPECT(assert_equal(expectedInequalities, *inequalities)); EXPECT(assert_equal(expectedInequalities, *inequalities));
bool isFeasible; bool isFeasible;
@ -414,7 +400,7 @@ TEST(QPSolver, failedSubproblem) {
qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2)));
qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0));
qp.inequalities.push_back( qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0), -ones(1), 0)); LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0), -1.0, 0));
VectorValues expected; VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0)); expected.insert(X(1), (Vector(2) << 1.0, 0.0));