LP.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_unstable/linear/LinearCost.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
 
#include <string>
 
namespace gtsam {
 
using namespace std;

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

struct LP {
  using shared_ptr = boost::shared_ptr<LP>;
 
  LinearCost cost; 
  EqualityFactorGraph equalities; 
  InequalityFactorGraph inequalities; 
private:
  mutable KeyDimMap cachedConstrainedKeyDimMap_; 
 
public:
  bool isFeasible(const VectorValues& x) const {
    return (equalities.error(x) == 0 && inequalities.error(x) == 0);
  }

  void print(const string& s = "") const {
    std::cout << s << std::endl;
    cost.print("Linear cost: ");
    equalities.print("Linear equality factors: ");
    inequalities.print("Linear inequality factors: ");
  }

  bool equals(const LP& other, double tol = 1e-9) const {
    return cost.equals(other.cost) && equalities.equals(other.equalities)
        && inequalities.equals(other.inequalities);
  }
 
  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;
  }
};

template<> struct traits<LP> : public Testable<LP> {
};
 
}