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

Merge remote-tracking branch 'origin/feature/LPSolver' into feature/LPSolver

release/4.3a0
= 2016-06-16 08:42:03 -04:00
parent 1102a0e425 2cc0d93468
commit 638e879577
8 changed files with 144 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -21,10 +21,7 @@ namespace gtsam {
class ActiveSetSolver {
protected:
KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities.
//!< used to initialize the working set factor graph,
//!< to which active inequalities will be added
VariableIndex costVariableIndex_, equalityVariableIndex_,
VariableIndex equalityVariableIndex_,
inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
public:
@ -92,4 +89,20 @@ protected:
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const;
};
/**
* Find the max key in a problem.
* Useful to determine unique keys for additional slack variables
*/
template <class PROBLEM>
Key maxKey(const PROBLEM& problem) {
auto keys = problem.cost.keys();
Key maxKey = *std::max_element(keys.begin(), keys.end());
if (!problem.equalities.empty())
maxKey = std::max(maxKey, *problem.equalities.keys().rbegin());
if (!problem.inequalities.empty())
maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin());
return maxKey;
}
} // namespace gtsam

43
gtsam_unstable/linear/LP.h
View File

@ -16,11 +16,36 @@ namespace gtsam {
using namespace std;
/// Mapping between variable's key and its corresponding dimensionality
using KeyDimMap = std::map<Key, size_t>;
/*
* Iterates through every factor in a linear graph and generates a
* mapping between every factor key and it's corresponding dimensionality.
*/
template <class LinearGraph>
KeyDimMap collectKeyDim(const LinearGraph& linearGraph) {
KeyDimMap keyDimMap;
for (const typename LinearGraph::sharedFactor& factor : linearGraph) {
if (!factor) continue;
for (Key key : factor->keys())
keyDimMap[key] = factor->getDim(factor->find(key));
}
return keyDimMap;
}
/**
* Data structure of a Linear Program
*/
struct LP {
using shared_ptr = boost::shared_ptr<LP>;
LinearCost cost; //!< Linear cost factor
EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0
InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0
private:
mutable KeyDimMap cachedConstrainedKeyDimMap_; //!< cached key-dim map of all variables in the constraints
public:
/// check feasibility
bool isFeasible(const VectorValues& x) const {
return (equalities.error(x) == 0 && inequalities.error(x) == 0);
@ -40,10 +65,26 @@ struct LP {
&& inequalities.equals(other.inequalities);
}
typedef boost::shared_ptr<LP> shared_ptr;
const KeyDimMap& constrainedKeyDimMap() const {
if (!cachedConstrainedKeyDimMap_.empty())
return cachedConstrainedKeyDimMap_;
// Collect key-dim map of all variables in the constraints
cachedConstrainedKeyDimMap_ = collectKeyDim(equalities);
KeyDimMap keysDim2 = collectKeyDim(inequalities);
cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end());
return cachedConstrainedKeyDimMap_;
}
Vector costGradient(Key key, const VectorValues& delta) const {
Vector g = Vector::Zero(delta.at(key).size());
Factor::const_iterator it = cost.find(key);
if (it != cost.end()) g = cost.getA(it).transpose();
return g;
}
};
/// traits
template<> struct traits<LP> : public Testable<LP> {
};
}

21
gtsam_unstable/linear/LPInitSolver.h
View File

@ -40,12 +40,10 @@ namespace gtsam {
*/
class LPInitSolver {
private:
const LPSolver& lpSolver_;
const LP& lp_;
public:
LPInitSolver(const LPSolver& lpSolver) :
lpSolver_(lpSolver), lp_(lpSolver.lp()) {
LPInitSolver(const LP& lp) : lp_(lp) {
}
virtual ~LPInitSolver() {
@ -56,7 +54,7 @@ public:
GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph();
VectorValues x0 = initOfInitGraph->optimize();
double y0 = compute_y0(x0);
Key yKey = maxKey(lpSolver_.keysDim()) + 1; // the unique key for y0
Key yKey = maxKey(lp_) + 1; // the unique key for y0
VectorValues xy0(x0);
xy0.insert(yKey, Vector::Constant(1, y0));
@ -85,15 +83,6 @@ private:
return initLP;
}
/// Find the max key in the problem to determine unique keys for additional slack variables
Key maxKey(const KeyDimMap& keysDim) const {
Key maxK = 0;
for (Key key : keysDim | boost::adaptors::map_keys)
if (maxK < key)
maxK = key;
return maxK;
}
/**
* Build the following graph to solve for an initial value of the initial problem
* min ||x||^2 s.t. Ax = b
@ -104,9 +93,9 @@ private:
new GaussianFactorGraph(lp_.equalities));
// create factor ||x||^2 and add to the graph
const KeyDimMap& keysDim = lpSolver_.keysDim();
for (Key key : keysDim | boost::adaptors::map_keys) {
size_t dim = keysDim.at(key);
const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
for (Key key : constrainedKeyDim | boost::adaptors::map_keys) {
size_t dim = constrainedKeyDim.at(key);
initGraph->push_back(
JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
}

72
gtsam_unstable/linear/LPSolver.cpp
View File

@ -12,19 +12,9 @@
#include <gtsam_unstable/linear/LPInitSolver.h>
namespace gtsam {
//******************************************************************************
LPSolver::LPSolver(const LP &lp) :
lp_(lp), addedZeroPriorsIndex_() {
// 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
baseGraph_.push_back(lp_.equalities);
// Collect key-dim map of all variables in the constraints to create their
// zero priors later
keysDim_ = collectKeysDim(lp_.equalities);
KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities);
keysDim_.insert(keysDim2.begin(), keysDim2.end());
lp_(lp) {
// Variable index
equalityVariableIndex_ = VariableIndex(lp_.equalities);
inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
@ -32,6 +22,7 @@ LPSolver::LPSolver(const LP &lp) :
constrainedKeys_.merge(lp_.inequalities.keys());
}
//******************************************************************************
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
@ -45,7 +36,7 @@ LPState LPSolver::iterate(const LPState &state) const {
// Compute lambda from the dual graph
// LP: project the objective's gradient onto each constraint gradient to
// obtain the dual scaling factors
// is it true??
// is it true??
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
@ -70,8 +61,8 @@ LPState LPSolver::iterate(const LPState &state) const {
// If we CAN make some progress, i.e. p_k != 0
// Adapt stepsize if some inactive constraints complain about this move
// LP: projection on nullspace is NOT zero:
// find and put a blocking inactive constraint to the working set,
// otherwise the problem is unbounded!!!
// find and put a blocking inactive constraint to the working set,
// otherwise the problem is unbounded!!!
double alpha;
int factorIx;
VectorValues p = newValues - state.values;
@ -89,6 +80,7 @@ LPState LPSolver::iterate(const LPState &state) const {
}
}
//******************************************************************************
GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
const LinearCost &cost, const VectorValues &xk) const {
GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph());
@ -101,30 +93,28 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
KeySet allKeys = lp_.inequalities.keys();
allKeys.merge(lp_.equalities.keys());
allKeys.merge(KeySet(lp_.cost.keys()));
// add the corresponding factors for all variables that are not explicitly in the
// cost function for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
// Add corresponding factors for all variables that are not explicitly in the
// cost function. Gradients of the cost function wrt to these variables are
// zero (g=0), so b=xk
if (cost.keys().size() != allKeys.size()) {
KeySet difference;
std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(),
lp_.cost.end(), std::inserter(difference, difference.end()));
for (Key k : difference) {
size_t dim = keysDim_.at(k);
size_t dim = lp_.constrainedKeyDimMap().at(k);
graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
}
}
return graph;
}
//******************************************************************************
VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
const InequalityFactorGraph &workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
// We remove the old zero priors from the base graph we are going to use to solve
//This iteration's problem
// for (size_t index : addedZeroPriorsIndex_) {
// workingGraph.remove(index);
// }
GaussianFactorGraph workingGraph;
// || X - Xk + g ||^2
workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
workingGraph.push_back(lp_.equalities);
for (const LinearInequality::shared_ptr &factor : workingSet) {
if (factor->active())
workingGraph.push_back(factor);
@ -132,37 +122,36 @@ VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
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 = Vector::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
Vector b = lp_.costGradient(key, delta);
// to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b);
} else {
return boost::make_shared<JacobianFactor>();
}
}
//******************************************************************************
InequalityFactorGraph LPSolver::identifyActiveConstraints(
const InequalityFactorGraph &inequalities,
const VectorValues &initialValues, const VectorValues &duals) const {
InequalityFactorGraph workingSet;
for (const LinearInequality::shared_ptr &factor : inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
GTSAM_PRINT(*workingFactor);
GTSAM_PRINT(initialValues);
double error = workingFactor->error(initialValues);
// TODO: find a feasible initial point for LPSolver.
// For now, we just throw an exception
@ -179,6 +168,7 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
return workingSet;
}
//******************************************************************************
std::pair<VectorValues, VectorValues> LPSolver::optimize(
const VectorValues &initialValues, const VectorValues &duals) const {
{
@ -195,6 +185,7 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
}
}
//******************************************************************************
boost::tuples::tuple<double, int> LPSolver::computeStepSize(
const InequalityFactorGraph &workingSet, const VectorValues &xk,
const VectorValues &p) const {
@ -202,8 +193,9 @@ boost::tuples::tuple<double, int> LPSolver::computeStepSize(
std::numeric_limits<double>::infinity());
}
//******************************************************************************
pair<VectorValues, VectorValues> LPSolver::optimize() const {
LPInitSolver initSolver(*this);
LPInitSolver initSolver(lp_);
VectorValues initValues = initSolver.solve();
return optimize(initValues);
}

27
gtsam_unstable/linear/LPSolver.h
View File

@ -18,12 +18,9 @@
namespace gtsam {
typedef std::map<Key, size_t> KeyDimMap;
class LPSolver: public ActiveSetSolver {
const LP &lp_; //!< the linear programming problem
KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors
std::vector<size_t> addedZeroPriorsIndex_;
public:
/// Constructor
LPSolver(const LP &lp);
@ -32,30 +29,6 @@ public:
return lp_;
}
const KeyDimMap &keysDim() const {
return keysDim_;
}
/*
* Iterates through every factor in a linear graph and generates a
* mapping between every factor key and it's corresponding dimensionality.
*/
template<class LinearGraph>
KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const {
KeyDimMap keysDim;
for (const typename LinearGraph::sharedFactor &factor : linearGraph) {
if (!factor)
continue;
for (Key key : factor->keys())
keysDim[key] = factor->getDim(factor->find(key));
}
return keysDim;
}
/// 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;
/*
* This function performs an iteration of the Active Set Method for solving
* LP problems. At the end of this iteration the problem should either be found

21
gtsam_unstable/linear/QP.h
View File

@ -33,6 +33,10 @@ struct QP {
EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0
InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0
private:
mutable VariableIndex cachedCostVariableIndex_;
public:
/** default constructor */
QP() :
cost(), equalities(), inequalities() {
@ -53,6 +57,23 @@ struct QP {
equalities.print("Linear equality factors: ");
inequalities.print("Linear inequality factors: ");
}
const VariableIndex& costVariableIndex() const {
if (cachedCostVariableIndex_.size() == 0)
cachedCostVariableIndex_ = VariableIndex(cost);
return cachedCostVariableIndex_;
}
Vector costGradient(Key key, const VectorValues& delta) const {
Vector g = Vector::Zero(delta.at(key).size());
if (costVariableIndex().find(key) != costVariableIndex().end()) {
for (size_t factorIx : costVariableIndex()[key]) {
GaussianFactor::shared_ptr factor = cost.at(factorIx);
g += factor->gradient(key, delta);
}
}
return g;
}
};
} // namespace gtsam

73
gtsam_unstable/linear/QPSolver.cpp
View File

@ -31,9 +31,6 @@ namespace gtsam {
//******************************************************************************
QPSolver::QPSolver(const QP& qp) :
qp_(qp) {
baseGraph_ = qp_.cost;
baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end());
costVariableIndex_ = VariableIndex(qp_.cost);
equalityVariableIndex_ = VariableIndex(qp_.equalities);
inequalityVariableIndex_ = VariableIndex(qp_.inequalities);
constrainedKeys_ = qp_.equalities.keys();
@ -43,7 +40,8 @@ QPSolver::QPSolver(const QP& qp) :
//***************************************************cc***************************
VectorValues QPSolver::solveWithCurrentWorkingSet(
const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_;
GaussianFactorGraph workingGraph = qp_.cost;
workingGraph.push_back(qp_.equalities);
for (const LinearInequality::shared_ptr& factor : workingSet) {
if (factor->active())
workingGraph.push_back(factor);
@ -52,28 +50,23 @@ VectorValues QPSolver::solveWithCurrentWorkingSet(
}
//******************************************************************************
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_);
TermsContainer Aterms = collectDualJacobians<LinearEquality>(
key, qp_.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 = Vector::Zero(delta.at(key).size());
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
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
Vector b = qp_.costGradient(key, delta);
// to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b);
} else {
return boost::make_shared<JacobianFactor>();
}
@ -139,25 +132,17 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints(
InequalityFactorGraph workingSet;
for (const LinearInequality::shared_ptr& factor : inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
workingFactor->activate();
if (useWarmStart && duals.size() > 0) {
if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
else workingFactor->inactivate();
} else {
if (useWarmStart == true && duals.size() > 0) {
double error = workingFactor->error(initialValues);
// Safety guard. This should not happen unless users provide a bad init
if (error > 0) throw InfeasibleInitialValues();
if (fabs(error) < 1e-7)
workingFactor->activate();
else
workingFactor->inactivate();
} else {
double error = workingFactor->error(initialValues);
// 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 (fabs(error) < 1e-7) {
workingFactor->activate();
} else {
workingFactor->inactivate();
}
}
}
workingSet.push_back(workingFactor);
}
@ -180,25 +165,19 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
return make_pair(state.values, state.duals);
}
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize() const {
//Make an LP with any linear cost function. It doesn't matter for initialization.
LP initProblem;
// make an unrelated key for a random variable cost: max key + 1
Key newKey = *qp_.cost.keys().rbegin();
if (!qp_.equalities.empty())
newKey = std::max(newKey, *qp_.equalities.keys().rbegin());
if (!qp_.inequalities.empty())
newKey = std::max(newKey, *qp_.inequalities.keys().rbegin());
++newKey;
// make an unrelated key for a random variable cost
Key newKey = maxKey(qp_) + 1;
initProblem.cost = LinearCost(newKey, Vector::Ones(1));
initProblem.equalities = qp_.equalities;
initProblem.inequalities = qp_.inequalities;
LPSolver solver(initProblem);
LPInitSolver initSolver(solver);
LPInitSolver initSolver(initProblem);
VectorValues initValues = initSolver.solve();
return optimize(initValues);
}
;
} /* namespace gtsam */

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

@ -106,8 +106,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
TEST(LPInitSolver, initialization) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
LPInitSolver initSolver(lpSolver);
LPInitSolver initSolver(lp);
GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph();
VectorValues x0 = initOfInitGraph->optimize();