ActiveSetSolver.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <boost/range/adaptor/map.hpp>

namespace gtsam {

template <class PROBLEM, class POLICY, class INITSOLVER>
class ActiveSetSolver {
public:
  struct State {
    VectorValues values;  
    VectorValues duals;   
    InequalityFactorGraph workingSet; 
    bool converged;     
    size_t iterations;  
    State()
        : values(), duals(), workingSet(), converged(false), iterations(0) {}

    State(const VectorValues& initialValues, const VectorValues& initialDuals,
          const InequalityFactorGraph& initialWorkingSet, bool _converged,
          size_t _iterations)
        : values(initialValues),
          duals(initialDuals),
          workingSet(initialWorkingSet),
          converged(_converged),
          iterations(_iterations) {}
  };

protected:
  const PROBLEM& problem_;  
  VariableIndex equalityVariableIndex_,
      inequalityVariableIndex_;  
  KeySet constrainedKeys_;  
  typedef std::vector<std::pair<Key, Matrix> > TermsContainer;

public:
  ActiveSetSolver(const PROBLEM& problem) :  problem_(problem) {
    equalityVariableIndex_ = VariableIndex(problem_.equalities);
    inequalityVariableIndex_ = VariableIndex(problem_.inequalities);
    constrainedKeys_ = problem_.equalities.keys();
    constrainedKeys_.merge(problem_.inequalities.keys());
  }

  std::pair<VectorValues, VectorValues> optimize(
      const VectorValues& initialValues,
      const VectorValues& duals = VectorValues(),
      bool useWarmStart = false) const;

  std::pair<VectorValues, VectorValues> optimize() const;

protected:
  boost::tuple<double, int> computeStepSize(
      const InequalityFactorGraph& workingSet, const VectorValues& xk,
      const VectorValues& p, const double& maxAlpha) const;

  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;
  }

  JacobianFactor::shared_ptr createDualFactor(
    Key key, const InequalityFactorGraph& workingSet,
    const VectorValues& delta) const;

public: 

  GaussianFactorGraph::shared_ptr buildDualGraph(
      const InequalityFactorGraph& workingSet, const VectorValues& delta) const;

  GaussianFactorGraph buildWorkingGraph(
      const InequalityFactorGraph& workingSet,
      const VectorValues& xk = VectorValues()) const;

  State iterate(const State& state) const;

  InequalityFactorGraph identifyActiveConstraints(
      const InequalityFactorGraph& inequalities,
      const VectorValues& initialValues,
      const VectorValues& duals = VectorValues(),
      bool useWarmStart = false) const;

  int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
      const VectorValues& lambdas) const;

};

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

#include <gtsam_unstable/linear/ActiveSetSolver-inl.h>