[FEATURE] LPSolver without initial Values.
[REFACTOR] Reformat code with eclipse code formatter.release/4.3a0
Ivan Jimenez
parent
8227f1a5fb
commit
ace23973a8
|
|
@ -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 {
|
||||
/**
|
||||
|
|
@ -39,13 +38,16 @@ namespace gtsam {
|
|||
* 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.
|
||||
*/
|
||||
class LPInitSolverMatlab : public LPInitSolver {
|
||||
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 {
|
||||
virtual VectorValues solve() const {
|
||||
// Build the graph to solve for the initial value of the initial problem
|
||||
GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph();
|
||||
VectorValues x0 = initOfInitGraph->optimize();
|
||||
|
|
@ -62,7 +64,7 @@ public:
|
|||
VectorValues xyInit = lpSolveInit.optimize(xy0).first;
|
||||
double yOpt = xyInit.at(yKey)[0];
|
||||
xyInit.erase(yKey);
|
||||
if ( yOpt > 0)
|
||||
if (yOpt > 0)
|
||||
throw InfeasibleOrUnboundedProblem();
|
||||
else
|
||||
return xyInit;
|
||||
|
|
@ -72,9 +74,10 @@ private:
|
|||
/// build initial LP
|
||||
LP::shared_ptr buildInitialLP(Key yKey) const {
|
||||
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->cost = LinearCost(yKey, ones(1)); // min y
|
||||
initLP->equalities = lp_.equalities; // st. Ax = b
|
||||
initLP->inequalities = addSlackVariableToInequalities(yKey,
|
||||
lp_.inequalities); // Cx-y <= d
|
||||
return initLP;
|
||||
}
|
||||
|
||||
|
|
@ -83,7 +86,7 @@ private:
|
|||
Key maxK = 0;
|
||||
BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys)
|
||||
if (maxK < key)
|
||||
maxK = key;
|
||||
maxK = key;
|
||||
return maxK;
|
||||
}
|
||||
|
||||
|
|
@ -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();
|
||||
|
|
@ -110,15 +114,15 @@ private:
|
|||
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) {
|
||||
double error = factor->error(x0);
|
||||
if (error > y0)
|
||||
y0 = error;
|
||||
y0 = error;
|
||||
}
|
||||
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> > terms;
|
||||
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)));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -37,7 +39,7 @@ LPSolver::LPSolver(const LP &lp) : lp_(lp) {
|
|||
GaussianFactorGraph::shared_ptr LPSolver::createZeroPriors(
|
||||
const KeyVector &costKeys, const KeyDimMap &keysDim) const {
|
||||
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()) {
|
||||
size_t dim = keysDim.at(key);
|
||||
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
|
||||
// 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
|
||||
|
|
@ -88,14 +92,16 @@ LPState LPSolver::iterate(const LPState &state) const {
|
|||
double alpha;
|
||||
int factorIx;
|
||||
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);
|
||||
// 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,36 +119,36 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
|
|||
return graph;
|
||||
}
|
||||
|
||||
VectorValues LPSolver::solveWithCurrentWorkingSet(
|
||||
const VectorValues &xk, const InequalityFactorGraph &workingSet) const {
|
||||
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
|
||||
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);
|
||||
for (const LinearInequality::shared_ptr &factor : workingSet) {
|
||||
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());
|
||||
AtermsInequalities.end());
|
||||
|
||||
// Collect the gradients of unconstrained cost factors to the b vector
|
||||
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>();
|
||||
}
|
||||
|
|
@ -152,13 +158,14 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
|
|||
const InequalityFactorGraph &inequalities,
|
||||
const VectorValues &initialValues, const VectorValues &duals) const {
|
||||
InequalityFactorGraph workingSet;
|
||||
for (const LinearInequality::shared_ptr &factor : inequalities) {
|
||||
for (const LinearInequality::shared_ptr &factor : inequalities) {
|
||||
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -21,18 +21,18 @@ namespace gtsam {
|
|||
typedef std::map<Key, size_t> KeyDimMap;
|
||||
|
||||
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
|
||||
|
||||
public:
|
||||
/// Constructor
|
||||
LPSolver(const LP& lp);
|
||||
LPSolver(const LP &lp);
|
||||
|
||||
const LP& lp() const {
|
||||
const LP &lp() const {
|
||||
return lp_;
|
||||
}
|
||||
|
||||
const KeyDimMap& keysDim() const {
|
||||
const KeyDimMap &keysDim() const {
|
||||
return keysDim_;
|
||||
}
|
||||
|
||||
|
|
@ -41,9 +41,9 @@ public:
|
|||
* mapping between every factor key and it's corresponding dimensionality.
|
||||
*/
|
||||
template<class LinearGraph>
|
||||
KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const {
|
||||
KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const {
|
||||
KeyDimMap keysDim;
|
||||
BOOST_FOREACH(const typename LinearGraph::sharedFactor& factor, linearGraph) {
|
||||
BOOST_FOREACH(const typename LinearGraph::sharedFactor &factor, linearGraph) {
|
||||
if (!factor) continue;
|
||||
BOOST_FOREACH(Key key, factor->keys())
|
||||
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
|
||||
GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector& costKeys,
|
||||
const KeyDimMap& keysDim) const;
|
||||
GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector &costKeys,
|
||||
const KeyDimMap &keysDim) const;
|
||||
|
||||
/*
|
||||
* 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
|
||||
* 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
|
||||
|
|
@ -76,11 +76,11 @@ public:
|
|||
* is the projection of the gradient onto the constraints' subspace
|
||||
*/
|
||||
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
|
||||
VectorValues solveWithCurrentWorkingSet(const VectorValues& xk,
|
||||
const InequalityFactorGraph& workingSet) const;
|
||||
VectorValues solveWithCurrentWorkingSet(const VectorValues &xk,
|
||||
const InequalityFactorGraph &workingSet) const;
|
||||
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
JacobianFactor::shared_ptr createDualFactor(Key key,
|
||||
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
|
||||
const InequalityFactorGraph &workingSet, const VectorValues &delta) const;
|
||||
|
||||
/// TODO(comment)
|
||||
boost::tuple<double, int> computeStepSize(
|
||||
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||
const VectorValues& p) const;
|
||||
const InequalityFactorGraph &workingSet, const VectorValues &xk,
|
||||
const VectorValues &p) const;
|
||||
|
||||
/*
|
||||
* 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
|
||||
*/
|
||||
InequalityFactorGraph identifyActiveConstraints(
|
||||
const InequalityFactorGraph& inequalities,
|
||||
const VectorValues& initialValues, const VectorValues& duals) const;
|
||||
const InequalityFactorGraph &inequalities,
|
||||
const VectorValues &initialValues, const VectorValues &duals) const;
|
||||
|
||||
/** Optimize with the provided feasible initial values
|
||||
* TODO: throw exception if the initial values is not feasible wrt inequality constraints
|
||||
* TODO: comment duals
|
||||
*/
|
||||
pair<VectorValues, VectorValues> optimize(const VectorValues& initialValues,
|
||||
const VectorValues& duals = VectorValues()) const;
|
||||
pair<VectorValues, VectorValues> optimize(const VectorValues &initialValues,
|
||||
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
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -46,21 +45,16 @@ 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.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
|
||||
return lp;
|
||||
}
|
||||
|
||||
LP infeasibleLP(){
|
||||
LP infeasibleLP() {
|
||||
LP lp;
|
||||
|
||||
lp.cost = LinearCost(1, Vector3(-1, -1, -2));
|
||||
|
|
@ -91,13 +85,13 @@ TEST(LPInitSolverMatlab, initialization) {
|
|||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
|
||||
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(
|
||||
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(
|
||||
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(
|
||||
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));
|
||||
|
||||
LPSolver lpSolveInit(*initLP);
|
||||
|
|
@ -122,61 +116,56 @@ TEST(LPInitSolverMatlab, initialization) {
|
|||
* x + 2y = 6
|
||||
*/
|
||||
TEST(LPSolver, overConstrainedLinearSystem) {
|
||||
GaussianFactorGraph graph;
|
||||
Matrix A1 = Vector3(1,1,1);
|
||||
Matrix A2 = Vector3(1,-1,2);
|
||||
Vector b = Vector3( 1, 5, 6);
|
||||
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
|
||||
graph.push_back(factor);
|
||||
GaussianFactorGraph graph;
|
||||
Matrix A1 = Vector3(1,1,1);
|
||||
Matrix A2 = Vector3(1,-1,2);
|
||||
Vector b = Vector3( 1, 5, 6);
|
||||
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
|
||||
graph.push_back(factor);
|
||||
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle over-constrained system
|
||||
CHECK(factor.error(x) != 0.0);
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle over-constrained system
|
||||
CHECK(factor.error(x) != 0.0);
|
||||
}
|
||||
|
||||
TEST(LPSolver, overConstrainedLinearSystem2) {
|
||||
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), 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)));
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle over-constrained system
|
||||
CHECK(graph.error(x) != 0.0);
|
||||
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), 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)));
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle over-constrained system
|
||||
CHECK(graph.error(x) != 0.0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, simpleTest1) {
|
||||
LP lp = simpleLP1();
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues init;
|
||||
init.insert(1, zero(2));
|
||||
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues init;
|
||||
init.insert(1, zero(2));
|
||||
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
|
||||
InequalityFactorGraph());
|
||||
VectorValues expected_x1;
|
||||
expected_x1.insert(1, Vector2( 1, 1));
|
||||
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
||||
|
||||
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
|
||||
InequalityFactorGraph());
|
||||
VectorValues expected_x1;
|
||||
expected_x1.insert(1, Vector2( 1, 1));
|
||||
CHECK(assert_equal(expected_x1, x1, 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));
|
||||
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) {
|
||||
// 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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -187,18 +176,18 @@ TEST(LPSolver, testWithoutInitialValues) {
|
|||
*/
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, LinearCost) {
|
||||
LinearCost cost(1, Vector3( 2., 4., 6.));
|
||||
VectorValues x;
|
||||
x.insert(1, Vector3( 1., 3., 5.));
|
||||
double error = cost.error(x);
|
||||
double expectedError = 44.0;
|
||||
DOUBLES_EQUAL(expectedError, error, 1e-100);
|
||||
LinearCost cost(1, Vector3( 2., 4., 6.));
|
||||
VectorValues x;
|
||||
x.insert(1, Vector3( 1., 3., 5.));
|
||||
double error = cost.error(x);
|
||||
double expectedError = 44.0;
|
||||
DOUBLES_EQUAL(expectedError, error, 1e-100);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue