refactor QPSolver to work with single-valued linear inequality factors. Unit tests passed.
thduynguyen
parent
6d68f5ffff
commit
a26ef7b7a2
|
|
@ -31,7 +31,10 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) {
|
|||
VectorValues QPSolver::solveWithCurrentWorkingSet(
|
||||
const LinearInequalityFactorGraph& workingSet) const {
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
@ -48,30 +51,19 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
|
|||
AtermsInequalities.end());
|
||||
|
||||
// Collect the gradients of unconstrained cost factors to the b vector
|
||||
Vector b = zero(delta.at(key).size());
|
||||
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
|
||||
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
|
||||
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
|
||||
b += factor->gradient(key, delta);
|
||||
if (Aterms.size() > 0) {
|
||||
Vector b = zero(delta.at(key).size());
|
||||
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
|
||||
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
|
||||
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()));
|
||||
}
|
||||
|
||||
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows()));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
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;
|
||||
else {
|
||||
return boost::make_shared<JacobianFactor>();
|
||||
}
|
||||
|
||||
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());
|
||||
BOOST_FOREACH(Key key, constrainedKeys_) {
|
||||
// Each constrained key becomes a factor in the dual graph
|
||||
dualGraph->push_back(createDualFactor(key, workingSet, delta));
|
||||
}
|
||||
|
||||
// Add prior for inactive dual variables
|
||||
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
|
||||
dualGraph->push_back(createDualPrior(factor));
|
||||
JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta);
|
||||
if (!dualFactor->empty())
|
||||
dualGraph->push_back(dualFactor);
|
||||
}
|
||||
return dualGraph;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
pair<int, int> QPSolver::identifyLeavingConstraint(
|
||||
int QPSolver::identifyLeavingConstraint(
|
||||
const LinearInequalityFactorGraph& workingSet,
|
||||
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
|
||||
// inactive or a good inequality constraint, so we don't care!
|
||||
double maxLambda = 0.0;
|
||||
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
|
||||
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
|
||||
Vector lambda = lambdas.at(factor->dualKey());
|
||||
Vector sigmas = factor->get_model()->sigmas();
|
||||
for (size_t j = 0; j < sigmas.size(); ++j)
|
||||
// If it is an active constraint, and lambda is larger than the current max
|
||||
if (sigmas[j] == ACTIVE && lambda[j] > maxLambda) {
|
||||
if (factor->active()) {
|
||||
double lambda = lambdas.at(factor->dualKey())[0];
|
||||
if (lambda > maxLambda) {
|
||||
worstFactorIx = factorIx;
|
||||
worstSigmaIx = j;
|
||||
maxLambda = lambda[j];
|
||||
maxLambda = lambda;
|
||||
}
|
||||
}
|
||||
}
|
||||
return make_pair(worstFactorIx, worstSigmaIx);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
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;
|
||||
return worstFactorIx;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -142,45 +116,43 @@ LinearInequalityFactorGraph QPSolver::updateWorkingSet(
|
|||
*
|
||||
* 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 VectorValues& p) const {
|
||||
static bool debug = false;
|
||||
|
||||
double minAlpha = 1.0;
|
||||
int closestFactorIx = -1, closestSigmaIx = -1;
|
||||
int closestFactorIx = -1;
|
||||
for(size_t factorIx = 0; factorIx<workingSet.size(); ++factorIx) {
|
||||
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
|
||||
Vector sigmas = factor->get_model()->sigmas();
|
||||
Vector b = factor->getb();
|
||||
for (size_t s = 0; s < sigmas.size(); ++s) {
|
||||
// If it is an inactive inequality, compute alpha and update min
|
||||
if (sigmas[s] == INACTIVE) {
|
||||
// Compute aj'*p
|
||||
double ajTp = factor->dotProductRow(s, p);
|
||||
double b = factor->getb()[0];
|
||||
// only check inactive factors
|
||||
if (!factor->active()) {
|
||||
// Compute a'*p
|
||||
double aTp = factor->dotProductRow(p);
|
||||
|
||||
// Check if aj'*p >0. Don't care if it's not.
|
||||
if (ajTp <= 0)
|
||||
continue;
|
||||
// Check if a'*p >0. Don't care if it's not.
|
||||
if (aTp <= 0)
|
||||
continue;
|
||||
|
||||
// Compute aj'*xk
|
||||
double ajTx = factor->dotProductRow(s, xk);
|
||||
// Compute a'*xk
|
||||
double aTx = factor->dotProductRow(xk);
|
||||
|
||||
// alpha = (bj - aj'*xk) / (aj'*p)
|
||||
double alpha = (b[s] - ajTx) / ajTp;
|
||||
if (debug)
|
||||
cout << "alpha: " << alpha << endl;
|
||||
// alpha = (b - a'*xk) / (a'*p)
|
||||
double alpha = (b - aTx) / aTp;
|
||||
if (debug)
|
||||
cout << "alpha: " << alpha << endl;
|
||||
|
||||
// We want the minimum of all those max alphas
|
||||
if (alpha < minAlpha) {
|
||||
closestFactorIx = factorIx;
|
||||
closestSigmaIx = s;
|
||||
minAlpha = alpha;
|
||||
}
|
||||
// We want the minimum of all those max alphas
|
||||
if (alpha < minAlpha) {
|
||||
closestFactorIx = factorIx;
|
||||
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)
|
||||
duals.print("Duals :");
|
||||
|
||||
int leavingFactor, leavingSigmaIx;
|
||||
boost::tie(leavingFactor, leavingSigmaIx) = //
|
||||
identifyLeavingConstraint(state.workingSet, duals);
|
||||
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
|
||||
if (debug)
|
||||
cout << "violated active inequality - factorIx, sigmaIx: " << leavingFactor
|
||||
<< " " << leavingSigmaIx << endl;
|
||||
cout << "leavingFactor: " << leavingFactor << endl;
|
||||
|
||||
// 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);
|
||||
}
|
||||
else {
|
||||
// Inactivate the leaving constraint
|
||||
LinearInequalityFactorGraph newWorkingSet = updateWorkingSet(
|
||||
state.workingSet, leavingFactor, leavingSigmaIx, INACTIVE);
|
||||
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
|
||||
newWorkingSet.at(leavingFactor)->inactivate();
|
||||
return QPState(newValues, duals, newWorkingSet, false);
|
||||
}
|
||||
}
|
||||
|
|
@ -226,16 +195,18 @@ QPState QPSolver::iterate(const QPState& state) const {
|
|||
// If we CAN make some progress
|
||||
// Adapt stepsize if some inactive constraints complain about this move
|
||||
double alpha;
|
||||
int factorIx, sigmaIx;
|
||||
int factorIx;
|
||||
VectorValues p = newValues - state.values;
|
||||
boost::tie(alpha, factorIx, sigmaIx) = //
|
||||
boost::tie(alpha, factorIx) = //
|
||||
computeStepSize(state.workingSet, state.values, p);
|
||||
if (debug)
|
||||
cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " "
|
||||
<< sigmaIx << endl;
|
||||
cout << "alpha, factorIx: " << alpha << " " << factorIx << " "
|
||||
<< endl;
|
||||
|
||||
// also add to the working set the one that complains the most
|
||||
LinearInequalityFactorGraph newWorkingSet = //
|
||||
updateWorkingSet(state.workingSet, factorIx, sigmaIx, ACTIVE);
|
||||
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
|
||||
if (factorIx >= 0)
|
||||
newWorkingSet.at(factorIx)->activate();
|
||||
|
||||
// step!
|
||||
newValues = state.values + alpha * p;
|
||||
|
|
@ -251,14 +222,12 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
|
|||
LinearInequalityFactorGraph workingSet;
|
||||
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
|
||||
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
|
||||
Vector e = workingFactor->error_vector(initialValues);
|
||||
Vector sigmas = zero(e.rows());
|
||||
for (int i = 0; i < e.rows(); ++i){
|
||||
if (fabs(e[i])>1e-7){
|
||||
sigmas[i] = INACTIVE;
|
||||
}
|
||||
double error = workingFactor->error(initialValues);
|
||||
if (fabs(error)>1e-7){
|
||||
workingFactor->inactivate();
|
||||
} else {
|
||||
workingFactor->activate();
|
||||
}
|
||||
workingFactor->setModel(true, sigmas);
|
||||
workingSet.push_back(workingFactor);
|
||||
}
|
||||
return workingSet;
|
||||
|
|
@ -410,13 +379,12 @@ boost::tuple<LinearEqualityFactorGraph::shared_ptr,
|
|||
// Add the slack term to the constraint
|
||||
// Unlike Nocedal06book, pg.473, we want ax-z <= b, since we always assume
|
||||
// LE constraints ax <= b.
|
||||
size_t dim = factor->rows();
|
||||
terms.push_back(make_pair(slackKey, -eye(dim)));
|
||||
inequalities->push_back(LinearInequality(terms, factor->getb(),
|
||||
terms.push_back(make_pair(slackKey, -eye(1)));
|
||||
inequalities->push_back(LinearInequality(terms, factor->getb()[0],
|
||||
factor->dualKey()));
|
||||
|
||||
// Add lower bound for this slack key
|
||||
slackLowerBounds.insert(slackKey, zero(dim));
|
||||
slackLowerBounds.insert(slackKey, zero(1));
|
||||
// Increase slackKey for the next slack variable
|
||||
slackKey++;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,9 +15,6 @@
|
|||
|
||||
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
|
||||
struct QPState {
|
||||
VectorValues values;
|
||||
|
|
@ -73,6 +70,7 @@ public:
|
|||
if (variableIndex.find(key) != variableIndex.end()) {
|
||||
BOOST_FOREACH(size_t factorIx, variableIndex[key]){
|
||||
typename FACTOR::shared_ptr factor = graph.at(factorIx);
|
||||
if (!factor->active()) continue;
|
||||
Matrix Ai = factor->getA(factor->find(key)).transpose();
|
||||
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
|
||||
}
|
||||
|
|
@ -85,15 +83,6 @@ public:
|
|||
const LinearInequalityFactorGraph& workingSet,
|
||||
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.
|
||||
*
|
||||
|
|
@ -161,17 +150,10 @@ public:
|
|||
* 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 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]
|
||||
*
|
||||
|
|
@ -180,7 +162,7 @@ public:
|
|||
* This constraint will be added to the working set and become active
|
||||
* in the next iteration
|
||||
*/
|
||||
boost::tuple<double, int, int> computeStepSize(
|
||||
boost::tuple<double, int> computeStepSize(
|
||||
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||
const VectorValues& p) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
|
||||
const Matrix One = ones(1,1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create test graph according to Forst10book_pg171Ex5
|
||||
QP createTestCase() {
|
||||
|
|
@ -40,12 +42,10 @@ QP createTestCase() {
|
|||
2.0 * ones(1, 1), zero(1), 10.0));
|
||||
|
||||
// Inequality constraints
|
||||
// Jacobian factors represent Ax-b, hence
|
||||
// x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
|
||||
Matrix A1 = (Matrix(4, 1) << 1, -1, 0, 1);
|
||||
Matrix A2 = (Matrix(4, 1) << 1, 0, -1, 0);
|
||||
Vector b = (Vector(4) << 2, 0, 0, 1.5);
|
||||
qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0));
|
||||
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
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -66,20 +66,20 @@ TEST(QPSolver, constraintsAux) {
|
|||
QPSolver solver(qp);
|
||||
|
||||
VectorValues lambdas;
|
||||
lambdas.insert(0, (Vector(4) << -0.5, 0.0, 0.3, 0.1));
|
||||
int factorIx, lambdaIx;
|
||||
boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint(
|
||||
qp.inequalities, lambdas);
|
||||
LONGS_EQUAL(0, factorIx);
|
||||
LONGS_EQUAL(2, lambdaIx);
|
||||
lambdas.insert(0, (Vector(1) << -0.5));
|
||||
lambdas.insert(1, (Vector(1) << 0.0));
|
||||
lambdas.insert(2, (Vector(1) << 0.3));
|
||||
lambdas.insert(3, (Vector(1) << 0.1));
|
||||
int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas);
|
||||
LONGS_EQUAL(2, factorIx);
|
||||
|
||||
VectorValues lambdas2;
|
||||
lambdas2.insert(0, (Vector(4) << -0.5, 0.0, -0.3, -0.1));
|
||||
int factorIx2, lambdaIx2;
|
||||
boost::tie(factorIx2, lambdaIx2) = solver.identifyLeavingConstraint(
|
||||
qp.inequalities, lambdas2);
|
||||
lambdas2.insert(0, (Vector(1) << -0.5));
|
||||
lambdas2.insert(1, (Vector(1) << 0.0));
|
||||
lambdas2.insert(2, (Vector(1) << -0.3));
|
||||
lambdas2.insert(3, (Vector(1) << -0.1));
|
||||
int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2);
|
||||
LONGS_EQUAL(-1, factorIx2);
|
||||
LONGS_EQUAL(-1, lambdaIx2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -135,9 +135,10 @@ TEST(QPSolver, indentifyActiveConstraints) {
|
|||
LinearInequalityFactorGraph workingSet =
|
||||
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
|
||||
|
||||
Vector actualSigmas = workingSet.at(0)->get_model()->sigmas();
|
||||
Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE);
|
||||
CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100));
|
||||
CHECK(!workingSet.at(0)->active()); // inactive
|
||||
CHECK(workingSet.at(1)->active()); // active
|
||||
CHECK(workingSet.at(2)->active()); // active
|
||||
CHECK(!workingSet.at(3)->active()); // inactive
|
||||
|
||||
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
|
||||
VectorValues expectedSolution;
|
||||
|
|
@ -159,19 +160,18 @@ TEST(QPSolver, iterate) {
|
|||
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
|
||||
expectedSolutions[0].insert(X(1), (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(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(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(2), (Vector(1) << 0.5));
|
||||
expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0));
|
||||
|
||||
LinearInequalityFactorGraph workingSet =
|
||||
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
|
||||
|
|
@ -223,12 +223,11 @@ QP createTestMatlabQPEx() {
|
|||
2.0 * ones(1, 1), 6 * ones(1), 1000.0));
|
||||
|
||||
// Inequality constraints
|
||||
// Jacobian factors represent Ax-b, hence
|
||||
// x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
|
||||
Matrix A1 = (Matrix(5, 1) << 1, -1, 2, -1, 0);
|
||||
Matrix A2 = (Matrix(5, 1) << 1, 2, 1, 0, -1);
|
||||
Vector b = (Vector(5) << 2, 2, 3, 0, 0);
|
||||
qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -256,16 +255,11 @@ QP createTestNocedal06bookEx16_4() {
|
|||
qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1)));
|
||||
|
||||
// Inequality constraints
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1),
|
||||
0));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1));
|
||||
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));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3));
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4));
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -317,16 +311,11 @@ QP modifyNocedal06bookEx16_4() {
|
|||
// Inequality constraints
|
||||
noiseModel::Constrained::shared_ptr noise =
|
||||
noiseModel::Constrained::MixedSigmas((Vector(1) << -1));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1),
|
||||
0));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1));
|
||||
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));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, -1, 0));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3));
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4));
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -360,18 +349,15 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
|
|||
|
||||
LinearInequalityFactorGraph expectedInequalities;
|
||||
expectedInequalities.push_back(
|
||||
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3),
|
||||
-ones(1, 1), -1 * ones(1), 0));
|
||||
LinearInequality(X(1), -One, X(2), 2 * One, X(3), -One, -1, 0));
|
||||
expectedInequalities.push_back(
|
||||
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4),
|
||||
-ones(1, 1), 6 * ones(1), 1));
|
||||
LinearInequality(X(1), One, X(2), 2 * One, X(4), -One, 6, 1));
|
||||
expectedInequalities.push_back(
|
||||
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5),
|
||||
-ones(1, 1), 2 * ones(1), 2));
|
||||
LinearInequality(X(1), One, X(2), -2 * One, X(5), -One, 2, 2));
|
||||
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(
|
||||
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));
|
||||
|
||||
bool isFeasible;
|
||||
|
|
@ -414,7 +400,7 @@ TEST(QPSolver, failedSubproblem) {
|
|||
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.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;
|
||||
expected.insert(X(1), (Vector(2) << 1.0, 0.0));
|
||||
|
|
|
|||
Loading…
Reference in New Issue