109 lines
3.6 KiB
C++
109 lines
3.6 KiB
C++
/**
|
|
* @file ActiveSetSolver.h
|
|
* @brief Abstract class above for solving problems with the abstract set method.
|
|
* @author Ivan Dario Jimenez
|
|
* @author Duy Nguyen Ta
|
|
* @date 1/25/16
|
|
*/
|
|
#pragma once
|
|
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
|
|
#include <boost/range/adaptor/map.hpp>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* This is a base class for all implementations of the active set algorithm for solving
|
|
* Programming problems. It provides services and variables all active set implementations
|
|
* share.
|
|
*/
|
|
class ActiveSetSolver {
|
|
protected:
|
|
KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
|
|
VariableIndex equalityVariableIndex_,
|
|
inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
|
|
|
|
public:
|
|
typedef std::vector<std::pair<Key, Matrix> > TermsContainer; //!< vector of key matrix pairs
|
|
//Matrices are usually the A term for a factor.
|
|
/**
|
|
* Creates a dual factor from the current workingSet and the key of the
|
|
* the variable used to created the dual factor.
|
|
*/
|
|
virtual JacobianFactor::shared_ptr createDualFactor(Key key,
|
|
const InequalityFactorGraph& workingSet,
|
|
const VectorValues& delta) const = 0;
|
|
|
|
/**
|
|
* Finds the active constraints in the given factor graph and returns the
|
|
* Dual Jacobians used to build a dual factor graph.
|
|
*/
|
|
template<typename FACTOR>
|
|
TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
|
|
const VariableIndex& variableIndex) const {
|
|
/*
|
|
* Iterates through each factor in the factor graph and checks
|
|
* whether it's active. If the factor is active it reutrns the A
|
|
* term of the factor.
|
|
*/
|
|
TermsContainer Aterms;
|
|
if (variableIndex.find(key) != variableIndex.end()) {
|
|
for (size_t factorIx : variableIndex[key]) {
|
|
typename FACTOR::shared_ptr factor = graph.at(factorIx);
|
|
if (!factor->active())
|
|
continue;
|
|
Matrix Ai = factor->getA(factor->find(key)).transpose();
|
|
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
|
|
}
|
|
}
|
|
return Aterms;
|
|
}
|
|
/**
|
|
* Identifies active constraints that shouldn't be active anymore.
|
|
*/
|
|
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
|
|
const VectorValues& lambdas) const;
|
|
|
|
/**
|
|
* Builds a dual graph from the current working set.
|
|
*/
|
|
GaussianFactorGraph::shared_ptr buildDualGraph(
|
|
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
|
|
|
|
protected:
|
|
/**
|
|
* Protected constructor because this class doesn't have any meaning without
|
|
* a concrete Programming problem to solve.
|
|
*/
|
|
ActiveSetSolver() :
|
|
constrainedKeys_() {
|
|
}
|
|
|
|
/**
|
|
* Computes the distance to move from the current point being examined to the next
|
|
* location to be examined by the graph. This should only be used where there are less
|
|
* than two constraints active.
|
|
*/
|
|
boost::tuple<double, int> computeStepSize(
|
|
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
|