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

36
gtsam_unstable/linear/LPInitSolverMatlab.h
View File

@ -5,12 +5,11 @@
* @date 1/24/16 * @date 1/24/16
*/ */
#pragma once #pragma once
#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 <CppUnitLite/Test.h>
namespace gtsam { namespace gtsam {
/** /**
@ -39,13 +38,16 @@ namespace gtsam {
* inequality constraint, we can't conclude that the problem is infeasible. * inequality constraint, we can't conclude that the problem is infeasible.
* However, whether it is infeasible or unbounded, we don't have a unique solution anyway. * However, whether it is infeasible or unbounded, we don't have a unique solution anyway.
*/ */
class LPInitSolverMatlab : public LPInitSolver { class LPInitSolverMatlab: public LPInitSolver {
typedef LPInitSolver Base; typedef LPInitSolver Base;
public: public:
LPInitSolverMatlab(const LPSolver& lpSolver) : Base(lpSolver) {} LPInitSolverMatlab(const LPSolver& lpSolver) :
virtual ~LPInitSolverMatlab() {} Base(lpSolver) {
}
virtual ~LPInitSolverMatlab() {
}
virtual VectorValues solve() const { virtual VectorValues solve() const {
// Build the graph to solve for the initial value of the initial problem // Build the graph to solve for the initial value of the initial problem
GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph(); GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph();
VectorValues x0 = initOfInitGraph->optimize(); VectorValues x0 = initOfInitGraph->optimize();
@ -62,7 +64,7 @@ public:
VectorValues xyInit = lpSolveInit.optimize(xy0).first; VectorValues xyInit = lpSolveInit.optimize(xy0).first;
double yOpt = xyInit.at(yKey)[0]; double yOpt = xyInit.at(yKey)[0];
xyInit.erase(yKey); xyInit.erase(yKey);
if ( yOpt > 0) if (yOpt > 0)
throw InfeasibleOrUnboundedProblem(); throw InfeasibleOrUnboundedProblem();
else else
return xyInit; return xyInit;
@ -72,9 +74,10 @@ private:
/// build initial LP /// build initial LP
LP::shared_ptr buildInitialLP(Key yKey) const { LP::shared_ptr buildInitialLP(Key yKey) const {
LP::shared_ptr initLP(new LP()); LP::shared_ptr initLP(new LP());
initLP->cost = LinearCost(yKey, ones(1)); // min y initLP->cost = LinearCost(yKey, ones(1)); // min y
initLP->equalities = lp_.equalities; // st. Ax = b 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; return initLP;
} }
@ -83,7 +86,7 @@ private:
Key maxK = 0; Key maxK = 0;
BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys)
if (maxK < key) if (maxK < key)
maxK = key; maxK = key;
return maxK; return maxK;
} }
@ -93,7 +96,8 @@ private:
*/ */
GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const { GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const {
// first add equality constraints Ax = b // 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 // create factor ||x||^2 and add to the graph
const KeyDimMap& keysDim = lpSolver_.keysDim(); const KeyDimMap& keysDim = lpSolver_.keysDim();
@ -110,15 +114,15 @@ private:
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) { BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) {
double error = factor->error(x0); double error = factor->error(x0);
if (error > y0) if (error > y0)
y0 = error; y0 = error;
} }
return y0; return y0;
} }
/// Collect all terms of a factor into a container. /// 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(
std::vector<std::pair<Key, Matrix> > terms; const LinearInequality& factor) const {
std::vector < std::pair<Key, Matrix> > terms;
for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
terms.push_back(make_pair(*it, factor.getA(it))); terms.push_back(make_pair(*it, factor.getA(it)));
} }

80
gtsam_unstable/linear/LPSolver.cpp
View File

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

59
gtsam_unstable/linear/LPSolver.h
View File

@ -21,18 +21,18 @@ namespace gtsam {
typedef std::map<Key, size_t> KeyDimMap; typedef std::map<Key, size_t> KeyDimMap;
class LPSolver: public ActiveSetSolver { class LPSolver: public ActiveSetSolver {
const LP& lp_; //!< the linear programming problem const LP &lp_; //!< the linear programming problem
KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors
public: public:
/// Constructor /// Constructor
LPSolver(const LP& lp); LPSolver(const LP &lp);
const LP& lp() const { const LP &lp() const {
return lp_; return lp_;
} }
const KeyDimMap& keysDim() const { const KeyDimMap &keysDim() const {
return keysDim_; return keysDim_;
} }
@ -41,9 +41,9 @@ public:
* mapping between every factor key and it's corresponding dimensionality. * mapping between every factor key and it's corresponding dimensionality.
*/ */
template<class LinearGraph> template<class LinearGraph>
KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const { KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const {
KeyDimMap keysDim; KeyDimMap keysDim;
BOOST_FOREACH(const typename LinearGraph::sharedFactor& factor, linearGraph) { BOOST_FOREACH(const typename LinearGraph::sharedFactor &factor, linearGraph) {
if (!factor) continue; if (!factor) continue;
BOOST_FOREACH(Key key, factor->keys()) BOOST_FOREACH(Key key, factor->keys())
keysDim[key] = factor->getDim(factor->find(key)); keysDim[key] = factor->getDim(factor->find(key));
@ -52,8 +52,8 @@ public:
} }
/// Create a zero prior for any keys in the graph that don't exist in the cost /// Create a zero prior for any keys in the graph that don't exist in the cost
GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector& costKeys, GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector &costKeys,
const KeyDimMap& keysDim) const; const KeyDimMap &keysDim) const;
/* /*
* This function performs an iteration of the Active Set Method for solving * This function performs an iteration of the Active Set Method for solving
@ -61,7 +61,7 @@ public:
* to be unfeasible, solved or the current state changed to reflect a new * to be unfeasible, solved or the current state changed to reflect a new
* working set. * working set.
*/ */
LPState iterate(const LPState& state) const; LPState iterate(const LPState &state) const;
/** /**
* Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution
@ -76,11 +76,11 @@ public:
* is the projection of the gradient onto the constraints' subspace * is the projection of the gradient onto the constraints' subspace
*/ */
GaussianFactorGraph::shared_ptr createLeastSquareFactors( GaussianFactorGraph::shared_ptr createLeastSquareFactors(
const LinearCost& cost, const VectorValues& xk) const; const LinearCost &cost, const VectorValues &xk) const;
/// Find solution with the current working set /// Find solution with the current working set
VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, VectorValues solveWithCurrentWorkingSet(const VectorValues &xk,
const InequalityFactorGraph& workingSet) const; const InequalityFactorGraph &workingSet) const;
/* /*
* A dual factor takes the objective function and a set of constraints. * A dual factor takes the objective function and a set of constraints.
@ -89,12 +89,12 @@ public:
* function g are dual factors and lambda is the lagrangian multiplier. * function g are dual factors and lambda is the lagrangian multiplier.
*/ */
JacobianFactor::shared_ptr createDualFactor(Key key, JacobianFactor::shared_ptr createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const VectorValues& delta) const; const InequalityFactorGraph &workingSet, const VectorValues &delta) const;
/// TODO(comment) /// TODO(comment)
boost::tuple<double, int> computeStepSize( boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk, const InequalityFactorGraph &workingSet, const VectorValues &xk,
const VectorValues& p) const; const VectorValues &p) const;
/* /*
* Given an initial value this function determine which constraints are active * Given an initial value this function determine which constraints are active
@ -102,34 +102,19 @@ public:
* A constraint Ax <= b is active if we have an x' s.t. Ax' = b * A constraint Ax <= b is active if we have an x' s.t. Ax' = b
*/ */
InequalityFactorGraph identifyActiveConstraints( InequalityFactorGraph identifyActiveConstraints(
const InequalityFactorGraph& inequalities, const InequalityFactorGraph &inequalities,
const VectorValues& initialValues, const VectorValues& duals) const; const VectorValues &initialValues, const VectorValues &duals) const;
/** Optimize with the provided feasible initial values /** Optimize with the provided feasible initial values
* TODO: throw exception if the initial values is not feasible wrt inequality constraints * TODO: throw exception if the initial values is not feasible wrt inequality constraints
* TODO: comment duals * TODO: comment duals
*/ */
pair<VectorValues, VectorValues> optimize(const VectorValues& initialValues, pair<VectorValues, VectorValues> optimize(const VectorValues &initialValues,
const VectorValues& duals = VectorValues()) const; const VectorValues &duals = VectorValues()) const;
/** /**
* Optimize without initial values * Optimize without initial values.
* TODO: Find a feasible initial solution that doesn't involve simplex method
* nor Solving another LP
*/ */
pair<VectorValues, VectorValues> optimize() const { 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);
}
}; };
} // namespace gtsam } // namespace gtsam

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

@ -31,7 +31,6 @@
#include <gtsam_unstable/linear/LPSolver.h> #include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h> #include <gtsam_unstable/linear/LPInitSolverMatlab.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand; using namespace gtsam::symbol_shorthand;
@ -46,21 +45,16 @@ using namespace gtsam::symbol_shorthand;
*/ */
LP simpleLP1() { LP simpleLP1() {
LP lp; LP lp;
lp.cost = LinearCost(1, Vector2( -1., -1.)); // min -x1-x2 (max x1+x2) lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.push_back( lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
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( lp.inequalities.push_back(LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
LinearInequality(1, Vector2( 0, -1), 0, 2)); // x2 >= 0 lp.inequalities.push_back(LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
lp.inequalities.push_back( lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
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; return lp;
} }
LP infeasibleLP(){ LP infeasibleLP() {
LP lp; LP lp;
lp.cost = LinearCost(1, Vector3(-1, -1, -2)); lp.cost = LinearCost(1, Vector3(-1, -1, -2));
@ -91,13 +85,13 @@ TEST(LPInitSolverMatlab, initialization) {
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( 0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 LinearInequality(1, Vector2( 0, -1), 2, Vector::Constant(1, -1), 0, 2));// -x2 - y <= 0
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( 1, 2), 2, Vector::Constant(1, -1), 4, 3)); // x1 + 2*x2 - y <= 4 LinearInequality(1, Vector2( 1, 2), 2, Vector::Constant(1, -1), 4, 3));// x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( 4, 2), 2, Vector::Constant(1, -1), 12, 4)); // 4x1 + 2x2 - y <= 12 LinearInequality(1, Vector2( 4, 2), 2, Vector::Constant(1, -1), 12, 4));// 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5)); // -x1 + x2 - y <= 1 LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5));// -x1 + x2 - y <= 1
CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
LPSolver lpSolveInit(*initLP); LPSolver lpSolveInit(*initLP);
@ -122,61 +116,56 @@ TEST(LPInitSolverMatlab, initialization) {
* x + 2y = 6 * x + 2y = 6
*/ */
TEST(LPSolver, overConstrainedLinearSystem) { TEST(LPSolver, overConstrainedLinearSystem) {
GaussianFactorGraph graph; GaussianFactorGraph graph;
Matrix A1 = Vector3(1,1,1); Matrix A1 = Vector3(1,1,1);
Matrix A2 = Vector3(1,-1,2); Matrix A2 = Vector3(1,-1,2);
Vector b = Vector3( 1, 5, 6); Vector b = Vector3( 1, 5, 6);
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
graph.push_back(factor); graph.push_back(factor);
VectorValues x = graph.optimize(); VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle over-constrained system // This check confirms that gtsam linear constraint solver can't handle over-constrained system
CHECK(factor.error(x) != 0.0); CHECK(factor.error(x) != 0.0);
} }
TEST(LPSolver, overConstrainedLinearSystem2) { TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph; GaussianFactorGraph graph;
graph.push_back(JacobianFactor(1, ones(1, 1), 2, ones(1, 1), ones(1), noiseModel::Constrained::All(1))); graph.push_back(JacobianFactor(1, ones(1, 1), 2, ones(1, 1), ones(1), noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, ones(1, 1), 2, -ones(1, 1), 5*ones(1), noiseModel::Constrained::All(1))); graph.push_back(JacobianFactor(1, ones(1, 1), 2, -ones(1, 1), 5*ones(1), noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, ones(1, 1), 2, 2*ones(1, 1), 6*ones(1), noiseModel::Constrained::All(1))); graph.push_back(JacobianFactor(1, ones(1, 1), 2, 2*ones(1, 1), 6*ones(1), noiseModel::Constrained::All(1)));
VectorValues x = graph.optimize(); VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle over-constrained system // This check confirms that gtsam linear constraint solver can't handle over-constrained system
CHECK(graph.error(x) != 0.0); CHECK(graph.error(x) != 0.0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LPSolver, simpleTest1) { TEST(LPSolver, simpleTest1) {
LP lp = simpleLP1(); LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues init;
init.insert(1, zero(2));
LPSolver lpSolver(lp); VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
VectorValues init; InequalityFactorGraph());
init.insert(1, zero(2)); VectorValues expected_x1;
expected_x1.insert(1, Vector2( 1, 1));
CHECK(assert_equal(expected_x1, x1, 1e-10));
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init, VectorValues result, duals;
InequalityFactorGraph()); boost::tie(result, duals) = lpSolver.optimize(init);
VectorValues expected_x1; VectorValues expectedResult;
expected_x1.insert(1, Vector2( 1, 1)); expectedResult.insert(1, Vector2(8./3., 2./3.));
CHECK(assert_equal(expected_x1, x1, 1e-10)); CHECK(assert_equal(expectedResult, result, 1e-10));
VectorValues result, duals;
boost::tie(result, duals) = lpSolver.optimize(init);
VectorValues expectedResult;
expectedResult.insert(1, Vector2(8./3., 2./3.));
CHECK(assert_equal(expectedResult, result, 1e-10));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) { TEST(LPSolver, testWithoutInitialValues) {
// LP lp = simpleLP1(); LP lp = simpleLP1();
// LPSolver lpSolver(lp);
// LPSolver lpSolver(lp); VectorValues result,duals, expectedResult;
// VectorValues result, duals; expectedResult.insert(1, Vector2(8./3., 2./3.));
// boost::tie(result, duals) = lpSolver.optimize(); boost::tie(result, duals) = lpSolver.optimize();
// CHECK(assert_equal(expectedResult, result));
// VectorValues expectedResult;
// expectedResult.insert(1, Vector2(8./3., 2./3.));
// CHECK(assert_equal(expectedResult, result, 1e-10));
} }
/** /**
@ -187,18 +176,18 @@ TEST(LPSolver, testWithoutInitialValues) {
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LPSolver, LinearCost) { TEST(LPSolver, LinearCost) {
LinearCost cost(1, Vector3( 2., 4., 6.)); LinearCost cost(1, Vector3( 2., 4., 6.));
VectorValues x; VectorValues x;
x.insert(1, Vector3( 1., 3., 5.)); x.insert(1, Vector3( 1., 3., 5.));
double error = cost.error(x); double error = cost.error(x);
double expectedError = 44.0; double expectedError = 44.0;
DOUBLES_EQUAL(expectedError, error, 1e-100); DOUBLES_EQUAL(expectedError, error, 1e-100);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */