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

[FEATURE] QPSolver without initial Values. 

[REFACTOR] Reformat code with eclipse code formatter.
release/4.3a0
Ivan Jimenez 2016-02-15 14:44:00 -05:00
parent ace23973a8
commit 3def6cab74
4 changed files with 87 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam_unstable/linear/LPInitSolver.h
View File

@ -1,5 +1,7 @@
#pragma once
#include "QPSolver.h"
namespace gtsam {
/**
* Abstract class to solve for an initial value of an LP problem
@ -13,6 +15,7 @@ public:
LPInitSolver(const LPSolver& lpSolver) :
lp_(lpSolver.lp()), lpSolver_(lpSolver) {
}
virtual ~LPInitSolver() {
}
;

1
gtsam_unstable/linear/LPInitSolverMatlab.h
View File

@ -9,6 +9,7 @@
#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <CppUnitLite/Test.h>
namespace gtsam {

68
gtsam_unstable/linear/QPSolver.cpp
View File

@ -22,13 +22,15 @@
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <boost/range/adaptor/map.hpp>
#include "LPInitSolverMatlab.h"
using namespace std;
namespace gtsam {
//******************************************************************************
QPSolver::QPSolver(const QP& qp) : qp_(qp) {
QPSolver::QPSolver(const QP& qp) :
qp_(qp) {
baseGraph_ = qp_.cost;
baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end());
costVariableIndex_ = VariableIndex(qp_.cost);
@ -43,23 +45,22 @@ VectorValues QPSolver::solveWithCurrentWorkingSet(
const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_;
for (const LinearInequality::shared_ptr& factor : workingSet) {
if (factor->active()) workingGraph.push_back(factor);
if (factor->active())
workingGraph.push_back(factor);
}
return workingGraph.optimize();
}
//******************************************************************************
JacobianFactor::shared_ptr QPSolver::createDualFactor(
Key key, const InequalityFactorGraph& workingSet,
const VectorValues& delta) const {
JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the
// dual key
std::vector<std::pair<Key, Matrix> > Aterms =
collectDualJacobians<LinearEquality>(key, qp_.equalities,
equalityVariableIndex_);
std::vector<std::pair<Key, Matrix> > AtermsInequalities =
collectDualJacobians<LinearInequality>(key, workingSet,
inequalityVariableIndex_);
std::vector < std::pair<Key, Matrix> > Aterms = collectDualJacobians
< LinearEquality > (key, qp_.equalities, equalityVariableIndex_);
std::vector < std::pair<Key, Matrix> > AtermsInequalities =
collectDualJacobians < LinearInequality
> (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end());
@ -67,13 +68,12 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(
if (Aterms.size() > 0) {
Vector b = zero(delta.at(key).size());
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
for (size_t factorIx: costVariableIndex_[key]) {
for (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); // compute the least-square approximation of dual variables
return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
} else {
return boost::make_shared<JacobianFactor>();
}
@ -97,8 +97,8 @@ QPState QPSolver::iterate(const QPState& state) const {
// update is zero.
if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph
GaussianFactorGraph::shared_ptr dualGraph =
buildDualGraph(state.workingSet, newValues);
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!!
@ -122,7 +122,8 @@ QPState QPSolver::iterate(const QPState& state) const {
computeStepSize(state.workingSet, state.values, p);
// also add to the working set the one that complains the most
InequalityFactorGraph newWorkingSet = state.workingSet;
if (factorIx >= 0) newWorkingSet.at(factorIx)->activate();
if (factorIx >= 0)
newWorkingSet.at(factorIx)->activate();
// step!
newValues = state.values + alpha * p;
return QPState(newValues, state.duals, newWorkingSet, false,
@ -136,7 +137,7 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints(
const VectorValues& initialValues, const VectorValues& duals,
bool useWarmStart) const {
InequalityFactorGraph workingSet;
for (const LinearInequality::shared_ptr& factor: inequalities) {
for (const LinearInequality::shared_ptr& factor : inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
workingFactor->activate();
@ -148,7 +149,8 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints(
// TODO: find a feasible initial point for QPSolver.
// For now, we just throw an exception, since we don't have an LPSolver
// to do this yet
if (error > 0) throw InfeasibleInitialValues();
if (error > 0)
throw InfeasibleInitialValues();
if (fabs(error) < 1e-7) {
workingFactor->activate();
@ -167,8 +169,8 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initialValues, const VectorValues& duals,
bool useWarmStart) const {
// Initialize workingSet from the feasible initialValues
InequalityFactorGraph workingSet = identifyActiveConstraints(
qp_.inequalities, initialValues, duals, useWarmStart);
InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities,
initialValues, duals, useWarmStart);
QPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver
@ -179,18 +181,22 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
}
pair<VectorValues, VectorValues> QPSolver::optimize() const {
//Make an LP with any linear cost function. It doesn't matter for initialization.
LP initProblem;
initProblem.cost = LinearCost(2,ones(1,1)); // min gamma s.t.
initProblem.equalities = qp_.equalities; // s.t Ax = b from the original problem
//TODO: SLACK Variable needs to be added
initProblem.inequalities = qp_.inequalities; // s.t. Ax - gamma <= b Ax from the original problem
Key newKey = 0; // make an unrelated key for a random variable cost
BOOST_FOREACH(Key key, qp_.cost.getKeyDimMap() | boost::adaptors::map_keys)
if(newKey < key)
newKey = key;
newKey++;
initProblem.cost = LinearCost(newKey, ones(1));
initProblem.equalities = qp_.equalities;
initProblem.inequalities = qp_.inequalities;
LPSolver solver(initProblem);
VectorValues result, duals;
boost::tie(result, duals) = solver.optimize();
LPInitSolverMatlab initSolver(solver);
VectorValues initValues = initSolver.solve();
return optimize(result);
};
return optimize(initValues);
}
;
} /* namespace gtsam */

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

@ -28,8 +28,7 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
const Matrix One = ones(1,1);
const Matrix One = ones(1, 1);
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
@ -47,10 +46,11 @@ QP createTestCase() {
2.0 * ones(1, 1), zero(1), 10));
// Inequality constraints
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
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;
}
@ -140,9 +140,9 @@ TEST(QPSolver, indentifyActiveConstraints) {
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->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;
@ -228,14 +228,15 @@ QP createTestMatlabQPEx() {
// Inequality constraints
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, 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;
}
///* ************************************************************************* */
TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
@ -252,15 +253,14 @@ TEST(QPSolver, optimizeMatlabEx) {
///* ************************************************************************* */
TEST(QPSolver, optimizeMatlabExNoinitials) {
// TODO: Enable this test when everything is fixed.
// QP qp = createTestMatlabQPEx();
// QPSolver solver(qp);
// VectorValues solution;
// boost::tie(solution, boost::tuples::ignore) = solver.optimize();
// VectorValues expectedSolution;
// expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
// expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
// CHECK(assert_equal(expectedSolution, solution, 1e-7));
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize();
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
@ -272,9 +272,12 @@ 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, 0));
qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1));
qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2, 2));
qp.inequalities.push_back(
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2, 0));
qp.inequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1));
qp.inequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2, 2));
qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0.0, 3));
qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0.0, 4));
@ -339,32 +342,6 @@ TEST(QPSolver, infeasibleInitial) {
);
}
/* ************************************************************************* */
TEST(QPSolver, infeasibleConstraints) {
QP qp;
// Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
// TODO: Test Fails because we if constraints aren't feasible, the initial point will not
// be feasible either. we need to check for infeasible constraints.
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
2.0 * ones(1, 1), zero(1), 5));
qp.inequalities.push_back(LinearInequality(X(1), -1.0 * ones(1,1), X(2), -1.0 * ones(1,1), 32, 0)); // x1 + x2 >= 32
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), 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
QPSolver solver(qp);
VectorValues init;
init.insert(X(1), (Vector(1) << 0.0).finished());
init.insert(X(2), (Vector(1) << 0.0).finished());
CHECK_EXCEPTION(
solver.optimize(init),
InfeasibleOrUnboundedProblem);
}
/* ************************************************************************* */
int main() {
TestResult tr;