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

Some refactoring, small edits, TODOs for Ivan

release/4.3a0
Frank Dellaert 2016-01-29 09:07:14 -08:00
parent 0af87e7298
commit 26a7647629
8 changed files with 262 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

171
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -2,6 +2,7 @@
* @file ActiveSetSolver.h * @file ActiveSetSolver.h
* @brief Abstract class above for solving problems with the abstract set method. * @brief Abstract class above for solving problems with the abstract set method.
* @author Ivan Dario Jimenez * @author Ivan Dario Jimenez
* @author Duy Nguyen Ta
* @date 1/25/16 * @date 1/25/16
*/ */
#pragma once #pragma once
@ -11,78 +12,39 @@
namespace gtsam { namespace gtsam {
class ActiveSetSolver { class ActiveSetSolver {
protected: public:
typedef std::vector<std::pair<Key, Matrix> > TermsContainer; typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
protected:
KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities.
//!< used to initialize the working set factor graph, //!< used to initialize the working set factor graph,
//!< to which active inequalities will be added //!< to which active inequalities will be added
VariableIndex costVariableIndex_, equalityVariableIndex_, VariableIndex costVariableIndex_, equalityVariableIndex_,
inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
ActiveSetSolver() :
constrainedKeys_() {
}
/**
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
*
* @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex)
* is the constraint that has minimum alpha, or (-1,-1) if alpha = 1.
* This constraint will be added to the working set and become active
* in the next iteration
*/
boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const {
double minAlpha = startAlpha;
int closestFactorIx = -1;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
double b = factor->getb()[0];
// only check inactive factors
if (!factor->active()) {
// Compute a'*p
double aTp = factor->dotProductRow(p);
// Check if a'*p >0. Don't care if it's not.
if (aTp <= 0)
continue;
// Compute a'*xk
double aTx = factor->dotProductRow(xk);
// alpha = (b - a'*xk) / (a'*p)
double alpha = (b - aTx) / aTp;
// We want the minimum of all those max alphas
if (alpha < minAlpha) {
closestFactorIx = factorIx;
minAlpha = alpha;
}
}
}
return boost::make_tuple(minAlpha, closestFactorIx);
}
public: public:
/// Create a dual factor /// Create a dual factor
virtual JacobianFactor::shared_ptr createDualFactor(Key key, virtual JacobianFactor::shared_ptr createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet,
const VectorValues& delta) const = 0; const VectorValues& delta) const = 0;
//****************************************************************************** /// Collect the Jacobian terms for a dual factor
/// Collect the Jacobian terms for a dual factor template <typename FACTOR>
template<typename FACTOR> TermsContainer collectDualJacobians(
TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR> &graph, Key key, const FactorGraph<FACTOR>& graph,
const VariableIndex &variableIndex) const { const VariableIndex& variableIndex) const {
TermsContainer Aterms; TermsContainer Aterms;
if (variableIndex.find(key) != variableIndex.end()) { if (variableIndex.find(key) != variableIndex.end()) {
BOOST_FOREACH(size_t factorIx, variableIndex[key]) { BOOST_FOREACH (size_t factorIx, variableIndex[key]) {
typename FACTOR::shared_ptr factor = graph.at(factorIx); typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active()) continue; if (!factor->active()) continue;
Matrix Ai = factor->getA(factor->find(key)).transpose(); Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
}
} }
return Aterms;
} }
return Aterms;
}
/** /**
* The goal of this function is to find currently active inequality constraints * The goal of this function is to find currently active inequality constraints
@ -118,36 +80,83 @@ public:
* And we want to remove the worst one with the largest lambda from the active set. * And we want to remove the worst one with the largest lambda from the active set.
* *
*/ */
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const { const VectorValues& lambdas) const {
int worstFactorIx = -1; int worstFactorIx = -1;
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is
// inactive or a good inequality constraint, so we don't care! // either
double maxLambda = 0.0; // inactive or a good inequality constraint, so we don't care!
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { double maxLambda = 0.0;
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
if (factor->active()) { const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
double lambda = lambdas.at(factor->dualKey())[0]; if (factor->active()) {
if (lambda > maxLambda) { double lambda = lambdas.at(factor->dualKey())[0];
worstFactorIx = factorIx; if (lambda > maxLambda) {
maxLambda = lambda; worstFactorIx = factorIx;
maxLambda = lambda;
}
} }
} }
return worstFactorIx;
} }
return worstFactorIx;
}
//****************************************************************************** /// TODO: comment
GaussianFactorGraph::shared_ptr buildDualGraph( GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet,
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); const VectorValues& delta) const {
BOOST_FOREACH(Key key, constrainedKeys_) { GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
// Each constrained key becomes a factor in the dual graph BOOST_FOREACH (Key key, constrainedKeys_) {
JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, // Each constrained key becomes a factor in the dual graph
delta); JacobianFactor::shared_ptr dualFactor =
if (!dualFactor->empty()) dualGraph->push_back(dualFactor); createDualFactor(key, workingSet, delta);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
}
return dualGraph;
} }
return dualGraph;
} protected:
ActiveSetSolver() : constrainedKeys_() {}
/**
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
*
* @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex)
* is the constraint that has minimum alpha, or (-1,-1) if alpha = 1.
* This constraint will be added to the working set and become active
* in the next iteration.
*/
boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const {
double minAlpha = startAlpha;
int closestFactorIx = -1;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
double b = factor->getb()[0];
// only check inactive factors
if (!factor->active()) {
// Compute a'*p
double aTp = factor->dotProductRow(p);
// Check if a'*p >0. Don't care if it's not.
if (aTp <= 0)
continue;
// Compute a'*xk
double aTx = factor->dotProductRow(xk);
// alpha = (b - a'*xk) / (a'*p)
double alpha = (b - aTx) / aTp;
// We want the minimum of all those max alphas
if (alpha < minAlpha) {
closestFactorIx = factorIx;
minAlpha = alpha;
}
}
}
return boost::make_tuple(minAlpha, closestFactorIx);
}
}; };
} } // namespace gtsam

3
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -18,8 +18,9 @@
#pragma once #pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam_unstable/linear/LinearInequality.h> #include <gtsam_unstable/linear/LinearInequality.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/FactorGraph.h>
namespace gtsam { namespace gtsam {

109
gtsam_unstable/linear/LPSolver.cpp
View File

@ -6,24 +6,24 @@
*/ */
#include <gtsam_unstable/linear/LPSolver.h> #include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h> #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam { namespace gtsam {
LPSolver::LPSolver(const LP &lp) : LPSolver::LPSolver(const LP &lp) : 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 not // Those include the equality constraints and zero priors for keys that are
// in the cost // not in the cost
baseGraph_.push_back(lp_.equalities); baseGraph_.push_back(lp_.equalities);
// Collect key-dim map of all variables in the constraints to create their zero priors later // Collect key-dim map of all variables in the constraints to create their
// zero priors later
keysDim_ = collectKeysDim(lp_.equalities); keysDim_ = collectKeysDim(lp_.equalities);
KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities); KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities);
keysDim_.insert(keysDim2.begin(), keysDim2.end()); keysDim_.insert(keysDim2.begin(), keysDim2.end());
// Create and push zero priors of constrained variables that do not exist in the cost function // Create and push zero priors of constrained variables that do not exist in
// the cost function
baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_)); baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_));
// Variable index // Variable index
@ -36,7 +36,7 @@ LPSolver::LPSolver(const 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());
BOOST_FOREACH(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)));
@ -47,35 +47,36 @@ GaussianFactorGraph::shared_ptr LPSolver::createZeroPriors(
LPState LPSolver::iterate(const LPState &state) const { LPState LPSolver::iterate(const LPState &state) const {
// Solve with the current working set // Solve with the current working set
// LP: project the objective neggradient 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 = solveWithCurrentWorkingSet(state.values, VectorValues newValues =
state.workingSet); solveWithCurrentWorkingSet(state.values, 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
if (newValues.equals(state.values, 1e-7)) { if (newValues.equals(state.values, 1e-7)) {
// Find and remove the bad ineq constraint by computing its lambda // Find and remove the bad inequality constraint by computing its lambda
// Compute lambda from the dual graph // Compute lambda from the dual graph
// LP: project the objective's gradient onto each constraint gradient to obtain the dual scaling factors // 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, GaussianFactorGraph::shared_ptr dualGraph =
newValues); buildDualGraph(state.workingSet, newValues);
VectorValues duals = dualGraph->optimize(); VectorValues duals = dualGraph->optimize();
// LP: see which ineq 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);
// If all inequality constraints are satisfied: We have the solution!! // If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) { if (leavingFactor < 0) {
// TODO If we still have infeasible equality constraints: the problem is over-constrained. No solution! // TODO If we still have infeasible equality constraints: the problem is
// over-constrained. No solution!
// ... // ...
return LPState(newValues, duals, state.workingSet, true, return LPState(newValues, duals, state.workingSet, true, state.iterations + 1);
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, return LPState(newValues, duals, newWorkingSet, false, state.iterations + 1);
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
@ -86,16 +87,14 @@ 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) if (factorIx >= 0) newWorkingSet.at(factorIx)->activate();
newWorkingSet.at(factorIx)->activate();
// step! // step!
newValues = state.values + alpha * p; newValues = state.values + alpha * p;
return LPState(newValues, state.duals, newWorkingSet, false, return LPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1);
state.iterations + 1);
} }
} }
@ -114,37 +113,35 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
} }
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));
BOOST_FOREACH(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()) if (it != lp_.cost.end()) b = lp_.cost.getA(it).transpose();
b = lp_.cost.getA(it).transpose(); 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>();
} }
@ -152,10 +149,9 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
InequalityFactorGraph LPSolver::identifyActiveConstraints( InequalityFactorGraph LPSolver::identifyActiveConstraints(
const InequalityFactorGraph &inequalities, const InequalityFactorGraph &inequalities,
const VectorValues &initialValues, const VectorValues &initialValues, const VectorValues &duals) const {
const VectorValues &duals) const {
InequalityFactorGraph workingSet; InequalityFactorGraph workingSet;
BOOST_FOREACH(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);
@ -165,8 +161,7 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
if (fabs(error) < 1e-7) { if (fabs(error) < 1e-7) {
workingFactor->activate(); workingFactor->activate();
} } else {
else {
workingFactor->inactivate(); workingFactor->inactivate();
} }
workingSet.push_back(workingFactor); workingSet.push_back(workingFactor);
@ -175,29 +170,25 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
} }
std::pair<VectorValues, VectorValues> LPSolver::optimize( std::pair<VectorValues, VectorValues> LPSolver::optimize(
const VectorValues &initialValues, const VectorValues &initialValues, const VectorValues &duals) const {
const VectorValues &duals) const {
{ {
// Initialize workingSet from the feasible initialValues // Initialize workingSet from the feasible initialValues
InequalityFactorGraph workingSet = identifyActiveConstraints( InequalityFactorGraph workingSet =
lp_.inequalities, initialValues, duals); identifyActiveConstraints(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
while (!state.converged) { while (!state.converged)
state = iterate(state); state = iterate(state);
}
return make_pair(state.values, state.duals); return make_pair(state.values, state.duals);
} }
} }
boost::tuples::tuple<double, int> LPSolver::computeStepSize( boost::tuples::tuple<double, int> LPSolver::computeStepSize(
const InequalityFactorGraph &workingSet, const InequalityFactorGraph &workingSet, const VectorValues &xk,
const VectorValues &xk,
const VectorValues &p) const { const VectorValues &p) const {
return ActiveSetSolver::computeStepSize(workingSet, xk, p, return ActiveSetSolver::computeStepSize(
std::numeric_limits<double>::infinity()); workingSet, xk, p, std::numeric_limits<double>::infinity());
} }
} }

26
gtsam_unstable/linear/LPSolver.h
View File

@ -2,6 +2,7 @@
* @file LPSolver.h * @file LPSolver.h
* @brief Class used to solve Linear Programming Problems as defined in LP.h * @brief Class used to solve Linear Programming Problems as defined in LP.h
* @author Ivan Dario Jimenez * @author Ivan Dario Jimenez
* @author Duy Nguyen Ta
* @date 1/24/16 * @date 1/24/16
*/ */
@ -10,9 +11,10 @@
#include <gtsam_unstable/linear/LPState.h> #include <gtsam_unstable/linear/LPState.h>
#include <gtsam_unstable/linear/LP.h> #include <gtsam_unstable/linear/LP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h> #include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <boost/range/adaptor/map.hpp>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/range/adaptor/map.hpp>
namespace gtsam { namespace gtsam {
typedef std::map<Key, size_t> KeyDimMap; typedef std::map<Key, size_t> KeyDimMap;
@ -28,11 +30,12 @@ public:
const LP& lp() const { const LP& lp() const {
return lp_; return lp_;
} }
const KeyDimMap& keysDim() const { const KeyDimMap& keysDim() const {
return keysDim_; return keysDim_;
} }
//****************************************************************************** /// TODO(comment)
template<class LinearGraph> template<class LinearGraph>
KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const { KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const {
KeyDimMap keysDim; KeyDimMap keysDim;
@ -44,17 +47,13 @@ public:
return keysDim; return keysDim;
} }
//****************************************************************************** /// 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;
//****************************************************************************** /// TODO(comment)
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
* on the constraint surface and g is the gradient of the linear cost, * on the constraint surface and g is the gradient of the linear cost,
@ -74,28 +73,27 @@ public:
VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, VectorValues solveWithCurrentWorkingSet(const VectorValues& xk,
const InequalityFactorGraph& workingSet) const; const InequalityFactorGraph& workingSet) const;
//****************************************************************************** /// TODO(comment)
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)
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;
//****************************************************************************** /// TODO(comment)
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
*/ */
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 wrt inequality constraints * TODO: Find a feasible initial solution wrt inequality constraints
@ -115,4 +113,4 @@ public:
// return make_pair(state.values, state.duals); // return make_pair(state.values, state.duals);
// } // }
}; };
} } // namespace gtsam

2
gtsam_unstable/linear/LPState.h
View File

@ -10,7 +10,9 @@
namespace gtsam { namespace gtsam {
// TODO: comment
struct LPState { struct LPState {
// TODO: comment member variables
VectorValues values; VectorValues values;
VectorValues duals; VectorValues duals;
InequalityFactorGraph workingSet; InequalityFactorGraph workingSet;

1
gtsam_unstable/linear/LinearInequality.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam { namespace gtsam {

192
gtsam_unstable/linear/QPSolver.cpp
View File

@ -27,8 +27,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
//****************************************************************************** //******************************************************************************
QPSolver::QPSolver(const QP& qp) : QPSolver::QPSolver(const QP& qp) : 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);
@ -42,39 +41,41 @@ QPSolver::QPSolver(const QP& qp) :
VectorValues QPSolver::solveWithCurrentWorkingSet( VectorValues QPSolver::solveWithCurrentWorkingSet(
const InequalityFactorGraph& workingSet) const { const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; GaussianFactorGraph workingGraph = baseGraph_;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { for (const LinearInequality::shared_ptr& factor : workingSet) {
if (factor->active()) if (factor->active()) workingGraph.push_back(factor);
workingGraph.push_back(factor);
} }
return workingGraph.optimize(); return workingGraph.optimize();
} }
//****************************************************************************** //******************************************************************************
JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, JacobianFactor::shared_ptr QPSolver::createDualFactor(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { Key key, const InequalityFactorGraph& workingSet,
const VectorValues& delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the dual key // Transpose the A matrix of constrained factors to have the jacobian of the
std::vector < std::pair<Key, Matrix> > Aterms = collectDualJacobians // dual key
< LinearEquality > (key, qp_.equalities, equalityVariableIndex_); std::vector<std::pair<Key, Matrix> > Aterms =
std::vector < std::pair<Key, Matrix> > AtermsInequalities = collectDualJacobians<LinearEquality>(key, qp_.equalities,
collectDualJacobians < LinearInequality equalityVariableIndex_);
> (key, workingSet, inequalityVariableIndex_); std::vector<std::pair<Key, Matrix> > AtermsInequalities =
collectDualJacobians<LinearInequality>(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());
if (costVariableIndex_.find(key) != costVariableIndex_.end()) { if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { for (size_t factorIx: costVariableIndex_[key]) {
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
b += factor->gradient(key, delta); b += factor->gradient(key, delta);
}
} }
return boost::make_shared<JacobianFactor>(
Aterms, b); // compute the least-square approximation of dual variables
} else {
return boost::make_shared<JacobianFactor>();
} }
return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
} else {
return boost::make_shared<JacobianFactor>();
}
} }
//****************************************************************************** //******************************************************************************
@ -94,102 +95,101 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
* We want the minimum of all those alphas among all inactive inequality. * We want the minimum of all those alphas among all inactive inequality.
*/ */
boost::tuple<double, int> QPSolver::computeStepSize( boost::tuple<double, int> QPSolver::computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk, const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const { const VectorValues& p) const {
return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1); return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1);
} }
//****************************************************************************** //******************************************************************************
QPState QPSolver::iterate(const QPState& state) const { QPState QPSolver::iterate(const QPState& state) const {
// Algorithm 16.3 from Nocedal06book. // Algorithm 16.3 from Nocedal06book.
// Solve with the current working set eqn 16.39, but instead of solving for p solve for x // Solve with the current working set eqn 16.39, but instead of solving for p
VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); // solve for x
// If we CAN'T move further VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet);
// if p_k = 0 is the original condition, modified by Duy to say that the state update is zero. // If we CAN'T move further
if (newValues.equals(state.values, 1e-7)) { // if p_k = 0 is the original condition, modified by Duy to say that the state
// Compute lambda from the dual graph // update is zero.
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, if (newValues.equals(state.values, 1e-7)) {
newValues); // Compute lambda from the dual graph
VectorValues duals = dualGraph->optimize(); GaussianFactorGraph::shared_ptr dualGraph =
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); buildDualGraph(state.workingSet, newValues);
// If all inequality constraints are satisfied: We have the solution!! VectorValues duals = dualGraph->optimize();
if (leavingFactor < 0) { int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
return QPState(newValues, duals, state.workingSet, true, // If all inequality constraints are satisfied: We have the solution!!
state.iterations + 1); if (leavingFactor < 0) {
return QPState(newValues, duals, state.workingSet, true,
state.iterations + 1);
} else {
// Inactivate the leaving constraint
InequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false,
state.iterations + 1);
}
} else { } else {
// Inactivate the leaving constraint // If we CAN make some progress, i.e. p_k != 0
// Adapt stepsize if some inactive constraints complain about this move
double alpha;
int factorIx;
VectorValues p = newValues - state.values;
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; InequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate(); if (factorIx >= 0) newWorkingSet.at(factorIx)->activate();
return QPState(newValues, duals, newWorkingSet, false, state.iterations + 1); // step!
newValues = state.values + alpha * p;
return QPState(newValues, state.duals, newWorkingSet, false,
state.iterations + 1);
} }
} else {
// If we CAN make some progress, i.e. p_k != 0
// Adapt stepsize if some inactive constraints complain about this move
double alpha;
int factorIx;
VectorValues p = newValues - state.values;
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();
// step!
newValues = state.values + alpha * p;
return QPState(newValues, state.duals, newWorkingSet, false,
state.iterations + 1);
}
} }
//****************************************************************************** //******************************************************************************
InequalityFactorGraph QPSolver::identifyActiveConstraints( InequalityFactorGraph QPSolver::identifyActiveConstraints(
const InequalityFactorGraph& inequalities, const VectorValues& initialValues, const InequalityFactorGraph& inequalities,
const VectorValues& duals, bool useWarmStart) const { const VectorValues& initialValues, const VectorValues& duals,
InequalityFactorGraph workingSet; bool useWarmStart) const {
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { InequalityFactorGraph workingSet;
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); for (const LinearInequality::shared_ptr& factor: inequalities) {
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
workingFactor->activate(); if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
} workingFactor->activate();
else {
if (useWarmStart == true && duals.size() > 0) {
workingFactor->inactivate();
} else { } else {
double error = workingFactor->error(initialValues); if (useWarmStart == true && duals.size() > 0) {
// 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(); 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);
} }
workingSet.push_back(workingFactor); return workingSet;
}
return workingSet;
} }
//****************************************************************************** //******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize( 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
InequalityFactorGraph workingSet = identifyActiveConstraints(
qp_.inequalities, initialValues, duals, useWarmStart);
QPState state(initialValues, duals, workingSet, false, 0);
// Initialize workingSet from the feasible initialValues /// main loop of the solver
InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities, while (!state.converged)
initialValues, duals, useWarmStart); state = iterate(state);
QPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver return make_pair(state.values, state.duals);
while (!state.converged) {
state = iterate(state);
}
return make_pair(state.values, state.duals);
} }
} /* namespace gtsam */ } /* namespace gtsam */

17
gtsam_unstable/linear/QPSolver.h
View File

@ -13,20 +13,22 @@
* @file QPSolver.h * @file QPSolver.h
* @brief A quadratic programming solver implements the active set method * @brief A quadratic programming solver implements the active set method
* @date Apr 15, 2014 * @date Apr 15, 2014
* @author Ivan Dario Jimenez
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
*/ */
#pragma once #pragma once
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/QP.h> #include <gtsam_unstable/linear/QP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h> #include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <gtsam_unstable/linear/QPState.h> #include <gtsam_unstable/linear/QPState.h>
#include <gtsam/linear/VectorValues.h>
#include <vector> #include <vector>
#include <set> #include <set>
namespace gtsam { namespace gtsam {
/** /**
* This QPSolver uses the active set method to solve a quadratic programming problem * This QPSolver uses the active set method to solve a quadratic programming problem
* defined in the QP struct. * defined in the QP struct.
@ -48,24 +50,23 @@ public:
/// Create a dual factor /// Create a dual factor
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)
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;
/** Iterate 1 step, return a new state with a new workingSet and values */ /// Iterate 1 step, return a new state with a new workingSet and values
QPState iterate(const QPState& state) const; QPState iterate(const QPState& state) const;
/** /// Identify active constraints based on initial values.
* Identify active constraints based on initial values.
*/
InequalityFactorGraph identifyActiveConstraints( InequalityFactorGraph identifyActiveConstraints(
const InequalityFactorGraph& inequalities, const InequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& duals = const VectorValues& initialValues, const VectorValues& duals =
VectorValues(), bool useWarmStart = true) const; VectorValues(), bool useWarmStart = true) const;
/** Optimize with a provided initial values /**
* Optimize with provided initial values
* For this version, it is the responsibility of the caller to provide * For this version, it is the responsibility of the caller to provide
* a feasible initial value, otherwise, an exception will be thrown. * a feasible initial value, otherwise, an exception will be thrown.
* @return a pair of <primal, dual> solutions * @return a pair of <primal, dual> solutions
@ -76,4 +77,4 @@ public:
}; };
} /* namespace gtsam */ } // namespace gtsam