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

[FEATURE] LPSolver without initial Values. 

[REFACTOR] Reformat code with eclipse code formatter.
release/4.3a0
Ivan Jimenez 2016-02-15 13:53:22 -05:00
parent 8227f1a5fb
commit ace23973a8
4 changed files with 144 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam_unstable/linear/LPInitSolverMatlab.h
View File

@ -5,12 +5,11 @@
* @date 1/24/16
*/
#pragma once
#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
#include <CppUnitLite/Test.h>
namespace gtsam {
/**
@ -42,8 +41,11 @@ namespace gtsam {
class LPInitSolverMatlab: public LPInitSolver {
typedef LPInitSolver Base;
public:
LPInitSolverMatlab(const LPSolver& lpSolver) : Base(lpSolver) {}
virtual ~LPInitSolverMatlab() {}
LPInitSolverMatlab(const LPSolver& lpSolver) :
Base(lpSolver) {
}
virtual ~LPInitSolverMatlab() {
}
virtual VectorValues solve() const {
// Build the graph to solve for the initial value of the initial problem
@ -74,7 +76,8 @@ private:
LP::shared_ptr initLP(new LP());
initLP->cost = LinearCost(yKey, ones(1)); // min y
initLP->equalities = lp_.equalities; // st. Ax = b
initLP->inequalities = addSlackVariableToInequalities(yKey, lp_.inequalities); // Cx-y <= d
initLP->inequalities = addSlackVariableToInequalities(yKey,
lp_.inequalities); // Cx-y <= d
return initLP;
}
@ -93,7 +96,8 @@ private:
*/
GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const {
// first add equality constraints Ax = b
GaussianFactorGraph::shared_ptr initGraph(new GaussianFactorGraph(lp_.equalities));
GaussianFactorGraph::shared_ptr initGraph(
new GaussianFactorGraph(lp_.equalities));
// create factor ||x||^2 and add to the graph
const KeyDimMap& keysDim = lpSolver_.keysDim();
@ -115,9 +119,9 @@ private:
return y0;
}
/// Collect all terms of a factor into a container.
std::vector<std::pair<Key, Matrix> > collectTerms(const LinearInequality& factor) const {
std::vector<std::pair<Key, Matrix> > collectTerms(
const LinearInequality& factor) const {
std::vector < std::pair<Key, Matrix> > terms;
for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
terms.push_back(make_pair(*it, factor.getA(it)));

68
gtsam_unstable/linear/LPSolver.cpp
View File

@ -9,9 +9,11 @@
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
namespace gtsam {
LPSolver::LPSolver(const LP &lp) : lp_(lp) {
LPSolver::LPSolver(const LP &lp) :
lp_(lp) {
// Push back factors that are the same in every iteration to the base graph.
// Those include the equality constraints and zero priors for keys that are
// not in the cost
@ -50,8 +52,8 @@ LPState LPSolver::iterate(const LPState &state) const {
// Solve with the current working set
// LP: project the objective neg. gradient to the constraint's null space
// to find the direction to move
VectorValues newValues =
solveWithCurrentWorkingSet(state.values, state.workingSet);
VectorValues newValues = solveWithCurrentWorkingSet(state.values,
state.workingSet);
// If we CAN'T move further
// LP: projection on the constraints' nullspace is zero: we are at a vertex
@ -61,8 +63,8 @@ LPState LPSolver::iterate(const LPState &state) const {
// LP: project the objective's gradient onto each constraint gradient to
// obtain the dual scaling factors
// is it true??
GaussianFactorGraph::shared_ptr dualGraph =
buildDualGraph(state.workingSet, newValues);
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
// LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
@ -71,13 +73,15 @@ LPState LPSolver::iterate(const LPState &state) const {
// TODO If we still have infeasible equality constraints: the problem is
// over-constrained. No solution!
// ...
return LPState(newValues, duals, state.workingSet, true, state.iterations + 1);
return LPState(newValues, duals, state.workingSet, true,
state.iterations + 1);
} else {
// Inactivate the leaving constraint
// LP: remove the bad ineq constraint out of the working set
InequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate();
return LPState(newValues, duals, newWorkingSet, false, state.iterations + 1);
return LPState(newValues, duals, newWorkingSet, false,
state.iterations + 1);
}
} else {
// If we CAN make some progress, i.e. p_k != 0
@ -92,10 +96,12 @@ LPState LPSolver::iterate(const LPState &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 LPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1);
return LPState(newValues, state.duals, newWorkingSet, false,
state.iterations + 1);
}
}
@ -113,26 +119,26 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
return graph;
}
VectorValues LPSolver::solveWithCurrentWorkingSet(
const VectorValues &xk, const InequalityFactorGraph &workingSet) const {
VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
const InequalityFactorGraph &workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
for (const LinearInequality::shared_ptr &factor : workingSet) {
if (factor->active()) workingGraph.push_back(factor);
if (factor->active())
workingGraph.push_back(factor);
}
return workingGraph.optimize();
}
boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
Key key, const InequalityFactorGraph &workingSet,
const VectorValues &delta) const {
boost::shared_ptr<JacobianFactor> LPSolver::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
TermsContainer Aterms = collectDualJacobians<LinearEquality>(
key, lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
key, workingSet, inequalityVariableIndex_);
TermsContainer Aterms = collectDualJacobians < LinearEquality
> (key, lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
> (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end());
@ -140,9 +146,9 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
if (Aterms.size() > 0) {
Vector b = zero(delta.at(key).size());
Factor::const_iterator it = lp_.cost.find(key);
if (it != lp_.cost.end()) b = lp_.cost.getA(it).transpose();
return boost::make_shared<JacobianFactor>(
Aterms, b); // compute the least-square approximation of dual variables
if (it != lp_.cost.end())
b = lp_.cost.getA(it).transpose();
return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
} else {
return boost::make_shared<JacobianFactor>();
}
@ -158,7 +164,8 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
double error = workingFactor->error(initialValues);
// TODO: find a feasible initial point for LPSolver.
// For now, we just throw an exception
if (error > 0) throw InfeasibleInitialValues();
if (error > 0)
throw InfeasibleInitialValues();
if (fabs(error) < 1e-7) {
workingFactor->activate();
@ -174,8 +181,8 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
const VectorValues &initialValues, const VectorValues &duals) const {
{
// Initialize workingSet from the feasible initialValues
InequalityFactorGraph workingSet =
identifyActiveConstraints(lp_.inequalities, initialValues, duals);
InequalityFactorGraph workingSet = identifyActiveConstraints(
lp_.inequalities, initialValues, duals);
LPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver
@ -189,7 +196,14 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
boost::tuples::tuple<double, int> LPSolver::computeStepSize(
const InequalityFactorGraph &workingSet, const VectorValues &xk,
const VectorValues &p) const {
return ActiveSetSolver::computeStepSize(
workingSet, xk, p, std::numeric_limits<double>::infinity());
return ActiveSetSolver::computeStepSize(workingSet, xk, p,
std::numeric_limits<double>::infinity());
}
pair<VectorValues, VectorValues> LPSolver::optimize() const {
LPInitSolverMatlab initSolver(*this);
VectorValues initValues = initSolver.solve();
return optimize(initValues);
}
}

19
gtsam_unstable/linear/LPSolver.h
View File

@ -113,23 +113,8 @@ public:
const VectorValues &duals = VectorValues()) const;
/**
* Optimize without initial values
* TODO: Find a feasible initial solution that doesn't involve simplex method
* nor Solving another LP
* Optimize without initial values.
*/
pair<VectorValues, VectorValues> optimize() const {
// Initialize workingSet from the feasible initialValues
// InequalityFactorGraph workingSet = identifyActiveConstraints(
// lp_.inequalities, initialValues, duals);
// LPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver
// while (!state.converged) {
// state = iterate(state);
// }
// return make_pair(state.values, state.duals);
}
pair<VectorValues, VectorValues> optimize() const;
};
} // namespace gtsam

33
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -31,7 +31,6 @@
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
@ -47,16 +46,11 @@ using namespace gtsam::symbol_shorthand;
LP simpleLP1() {
LP lp;
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.push_back(
LinearInequality(1, Vector2( -1, 0), 0, 1)); // x1 >= 0
lp.inequalities.push_back(
LinearInequality(1, Vector2( 0, -1), 0, 2)); // x2 >= 0
lp.inequalities.push_back(
LinearInequality(1, Vector2( 1, 2), 4, 3)); // x1 + 2*x2 <= 4
lp.inequalities.push_back(
LinearInequality(1, Vector2( 4, 2), 12, 4)); // 4x1 + 2x2 <= 12
lp.inequalities.push_back(
LinearInequality(1, Vector2( -1, 1), 1, 5)); // -x1 + x2 <= 1
lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
lp.inequalities.push_back(LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0
lp.inequalities.push_back(LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
lp.inequalities.push_back(LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
return lp;
}
@ -147,7 +141,6 @@ TEST(LPSolver, overConstrainedLinearSystem2) {
/* ************************************************************************* */
TEST(LPSolver, simpleTest1) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues init;
init.insert(1, zero(2));
@ -165,18 +158,14 @@ TEST(LPSolver, simpleTest1) {
CHECK(assert_equal(expectedResult, result, 1e-10));
}
/* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) {
// LP lp = simpleLP1();
//
// LPSolver lpSolver(lp);
// VectorValues result, duals;
// boost::tie(result, duals) = lpSolver.optimize();
//
// VectorValues expectedResult;
// expectedResult.insert(1, Vector2(8./3., 2./3.));
// CHECK(assert_equal(expectedResult, result, 1e-10));
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues result,duals, expectedResult;
expectedResult.insert(1, Vector2(8./3., 2./3.));
boost::tie(result, duals) = lpSolver.optimize();
CHECK(assert_equal(expectedResult, result));
}
/**