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

1
gtsam_unstable/linear/LPInitSolverMatlab.h
View File

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

62
gtsam_unstable/linear/QPSolver.cpp
View File

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

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

@ -30,7 +30,6 @@ using namespace gtsam::symbol_shorthand;
const Matrix One = ones(1, 1); const Matrix One = ones(1, 1);
/* ************************************************************************* */ /* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5 // Create test graph according to Forst10book_pg171Ex5
QP createTestCase() { QP createTestCase() {
@ -47,7 +46,8 @@ QP createTestCase() {
2.0 * ones(1, 1), zero(1), 10)); 2.0 * ones(1, 1), zero(1), 10));
// Inequality constraints // 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), 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(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(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), 1.5, 3)); // x1 <= 3/2
@ -236,6 +236,7 @@ QP createTestMatlabQPEx() {
return qp; return qp;
} }
///* ************************************************************************* */
TEST(QPSolver, optimizeMatlabEx) { TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx(); QP qp = createTestMatlabQPEx();
QPSolver solver(qp); QPSolver solver(qp);
@ -252,15 +253,14 @@ TEST(QPSolver, optimizeMatlabEx) {
///* ************************************************************************* */ ///* ************************************************************************* */
TEST(QPSolver, optimizeMatlabExNoinitials) { TEST(QPSolver, optimizeMatlabExNoinitials) {
// TODO: Enable this test when everything is fixed. QP qp = createTestMatlabQPEx();
// QP qp = createTestMatlabQPEx(); QPSolver solver(qp);
// QPSolver solver(qp); VectorValues solution;
// VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize();
// boost::tie(solution, boost::tuples::ignore) = solver.optimize(); VectorValues expectedSolution;
// VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
// expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
// expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); CHECK(assert_equal(expectedSolution, solution, 1e-7));
// 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))); qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1)));
// Inequality constraints // Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2, 0)); qp.inequalities.push_back(
qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1)); 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), 2, 2)); 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(1), -ones(1, 1), 0.0, 3));
qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0.0, 4)); 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() { int main() {
TestResult tr; TestResult tr;