refactor maxKey and keyDimMap out of solvers

gtsam_unstable/linear/ActiveSetSolver.h

@@ -92,4 +92,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) {
+  Key maxKey =
+      *std::max_element(problem.cost.keys().begin(), problem.cost.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

gtsam_unstable/linear/LP.h

@@ -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,19 @@ 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_;
+  }
 };
 
 /// traits
 template<> struct traits<LP> : public Testable<LP> {
 };
+
 }

gtsam_unstable/linear/LPInitSolver.h

@@ -41,12 +41,10 @@ namespace gtsam {
  */
 class LPInitSolver {
 private:
-  const LPSolver& lpSolver_;
   const LP& lp_;
 
 public:
-  LPInitSolver(const LPSolver& lpSolver) :
+  LPInitSolver(const LP& lp) : lp_(lp) {
-      lpSolver_(lpSolver), lp_(lpSolver.lp()) {
   }
 
   virtual ~LPInitSolver() {
@@ -57,7 +55,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));
 
@@ -86,15 +84,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
@@ -105,9 +94,9 @@ private:
         new GaussianFactorGraph(lp_.equalities));
 
     // create factor ||x||^2 and add to the graph
-    const KeyDimMap& keysDim = lpSolver_.keysDim();
+    const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
-    for (Key key : keysDim | boost::adaptors::map_keys) {
+    for (Key key : constrainedKeyDim | boost::adaptors::map_keys) {
-      size_t dim = keysDim.at(key);
+      size_t dim = constrainedKeyDim.at(key);
       initGraph->push_back(
           JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
     }

gtsam_unstable/linear/LPSolver.cpp

@@ -19,12 +19,6 @@ LPSolver::LPSolver(const LP &lp) :
   // 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());
-
   // Variable index
   equalityVariableIndex_ = VariableIndex(lp_.equalities);
   inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
@@ -101,14 +95,15 @@ 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
+  // Add 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
+  // 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)));
     }
   }
@@ -203,7 +198,7 @@ boost::tuples::tuple<double, int> LPSolver::computeStepSize(
 }
 
 pair<VectorValues, VectorValues> LPSolver::optimize() const {
-  LPInitSolver initSolver(*this);
+  LPInitSolver initSolver(lp_);
   VectorValues initValues = initSolver.solve();
   return optimize(initValues);
 }

gtsam_unstable/linear/LPSolver.h

@@ -18,11 +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
@@ -32,30 +30,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

gtsam_unstable/linear/QPSolver.cpp

@@ -172,25 +172,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
+  // make an unrelated key for a random variable cost
-  Key newKey = *qp_.cost.keys().rbegin();
+  Key newKey = maxKey(qp_) + 1;
-  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;
   initProblem.cost = LinearCost(newKey, Vector::Ones(1));
   initProblem.equalities = qp_.equalities;
   initProblem.inequalities = qp_.inequalities;
-  LPSolver solver(initProblem);
+  LPInitSolver initSolver(initProblem);
-  LPInitSolver initSolver(solver);
   VectorValues initValues = initSolver.solve();
 
   return optimize(initValues);
 }
-;
 
 } /* namespace gtsam */

gtsam_unstable/linear/tests/testLPSolver.cpp

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