more refactoring to make QPSolver and LPSolver more similar

gtsam_unstable/linear/ActiveSetSolver.h

@@ -21,10 +21,7 @@ namespace gtsam {
 class ActiveSetSolver {
 protected:
   KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
-  GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities.
+  VariableIndex equalityVariableIndex_,
-  //!< used to initialize the working set factor graph,
-  //!< to which active inequalities will be added
-  VariableIndex costVariableIndex_, equalityVariableIndex_,
       inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
 
 public:

gtsam_unstable/linear/LP.h

@@ -74,6 +74,13 @@ public:
     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;
+  }
 };
 
 /// traits

gtsam_unstable/linear/LPSolver.cpp

@@ -12,13 +12,9 @@
 #include <gtsam_unstable/linear/LPInitSolver.h>
 
 namespace gtsam {
+//******************************************************************************
 LPSolver::LPSolver(const LP &lp) :
-    lp_(lp), addedZeroPriorsIndex_() {
+    lp_(lp) {
-  // 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 in the cost
-  baseGraph_.push_back(lp_.equalities);
-
   // Variable index
   equalityVariableIndex_ = VariableIndex(lp_.equalities);
   inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
@@ -26,6 +22,7 @@ LPSolver::LPSolver(const LP &lp) :
   constrainedKeys_.merge(lp_.inequalities.keys());
 }
 
+//******************************************************************************
 LPState LPSolver::iterate(const LPState &state) const {
   // Solve with the current working set
   // LP: project the objective neg. gradient to the constraint's null space
@@ -39,7 +36,7 @@ LPState LPSolver::iterate(const LPState &state) const {
     // Compute lambda from the dual graph
     // 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,
         newValues);
     VectorValues duals = dualGraph->optimize();
@@ -64,8 +61,8 @@ LPState LPSolver::iterate(const LPState &state) const {
     // If we CAN make some progress, i.e. p_k != 0
     // Adapt stepsize if some inactive constraints complain about this move
     // LP: projection on nullspace is NOT zero:
-    // 		find and put a blocking inactive constraint to the working set,
+    //    find and put a blocking inactive constraint to the working set,
-    // 		otherwise the problem is unbounded!!!
+    //    otherwise the problem is unbounded!!!
     double alpha;
     int factorIx;
     VectorValues p = newValues - state.values;
@@ -83,6 +80,7 @@ LPState LPSolver::iterate(const LPState &state) const {
   }
 }
 
+//******************************************************************************
 GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
     const LinearCost &cost, const VectorValues &xk) const {
   GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph());
@@ -110,16 +108,13 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
   return graph;
 }
 
+//******************************************************************************
 VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
     const InequalityFactorGraph &workingSet) const {
-  GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
+  GaussianFactorGraph workingGraph;
-//  We remove the old zero priors from the base graph we are going to use to solve
+  // || X - Xk + g ||^2
-  //This iteration's problem
-//  for (size_t index : addedZeroPriorsIndex_) {
-//    workingGraph.remove(index);
-//  }
-
   workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
+  workingGraph.push_back(lp_.equalities);
   for (const LinearInequality::shared_ptr &factor : workingSet) {
     if (factor->active())
       workingGraph.push_back(factor);
@@ -127,37 +122,36 @@ VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
   return workingGraph.optimize();
 }
 
-boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
+//******************************************************************************
-    const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
+boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
+    Key key, const InequalityFactorGraph &workingSet,
+    const VectorValues &delta) const {
   // Transpose the A matrix of constrained factors to have the jacobian of the
   // 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(),
-      AtermsInequalities.end());
+                AtermsInequalities.end());
 
   // Collect the gradients of unconstrained cost factors to the b vector
   if (Aterms.size() > 0) {
-    Vector b = Vector::Zero(delta.at(key).size());
+    Vector b = lp_.costGradient(key, delta);
-    Factor::const_iterator it = lp_.cost.find(key);
+    // to compute the least-square approximation of dual variables
-    if (it != lp_.cost.end())
+    return boost::make_shared<JacobianFactor>(Aterms, b);
-      b = lp_.cost.getA(it).transpose();
-    return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
   } else {
     return boost::make_shared<JacobianFactor>();
   }
 }
 
+//******************************************************************************
 InequalityFactorGraph LPSolver::identifyActiveConstraints(
     const InequalityFactorGraph &inequalities,
     const VectorValues &initialValues, const VectorValues &duals) const {
   InequalityFactorGraph workingSet;
   for (const LinearInequality::shared_ptr &factor : inequalities) {
     LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-    GTSAM_PRINT(*workingFactor);
-    GTSAM_PRINT(initialValues);
     double error = workingFactor->error(initialValues);
     // TODO: find a feasible initial point for LPSolver.
     // For now, we just throw an exception
@@ -174,6 +168,7 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
   return workingSet;
 }
 
+//******************************************************************************
 std::pair<VectorValues, VectorValues> LPSolver::optimize(
     const VectorValues &initialValues, const VectorValues &duals) const {
   {
@@ -190,6 +185,7 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
   }
 }
 
+//******************************************************************************
 boost::tuples::tuple<double, int> LPSolver::computeStepSize(
     const InequalityFactorGraph &workingSet, const VectorValues &xk,
     const VectorValues &p) const {
@@ -197,6 +193,7 @@ boost::tuples::tuple<double, int> LPSolver::computeStepSize(
       std::numeric_limits<double>::infinity());
 }
 
+//******************************************************************************
 pair<VectorValues, VectorValues> LPSolver::optimize() const {
   LPInitSolver initSolver(lp_);
   VectorValues initValues = initSolver.solve();

gtsam_unstable/linear/LPSolver.h

@@ -21,7 +21,6 @@ namespace gtsam {
 
 class LPSolver: public ActiveSetSolver {
   const LP &lp_; //!< the linear programming problem
-  std::vector<size_t> addedZeroPriorsIndex_;
 public:
   /// Constructor
   LPSolver(const LP &lp);

gtsam_unstable/linear/QP.h

@@ -33,6 +33,10 @@ struct QP {
   EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0
   InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0
 
+private:
+  mutable VariableIndex cachedCostVariableIndex_;
+
+public:
   /** default constructor */
   QP() :
       cost(), equalities(), inequalities() {
@@ -53,6 +57,23 @@ struct QP {
     equalities.print("Linear equality factors: ");
     inequalities.print("Linear inequality factors: ");
   }
+
+  const VariableIndex& costVariableIndex() const {
+    if (cachedCostVariableIndex_.size() == 0)
+      cachedCostVariableIndex_ = VariableIndex(cost);
+    return cachedCostVariableIndex_;
+  }
+
+  Vector costGradient(Key key, const VectorValues& delta) const {
+    Vector g = Vector::Zero(delta.at(key).size());
+    if (costVariableIndex().find(key) != costVariableIndex().end()) {
+      for (size_t factorIx : costVariableIndex()[key]) {
+        GaussianFactor::shared_ptr factor = cost.at(factorIx);
+        g += factor->gradient(key, delta);
+      }
+    }
+    return g;
+  }
 };
 
 } // namespace gtsam

gtsam_unstable/linear/QPSolver.cpp

@@ -31,9 +31,6 @@ namespace gtsam {
 //******************************************************************************
 QPSolver::QPSolver(const QP& qp) :
     qp_(qp) {
-  baseGraph_ = qp_.cost;
-  baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end());
-  costVariableIndex_ = VariableIndex(qp_.cost);
   equalityVariableIndex_ = VariableIndex(qp_.equalities);
   inequalityVariableIndex_ = VariableIndex(qp_.inequalities);
   constrainedKeys_ = qp_.equalities.keys();
@@ -43,7 +40,8 @@ QPSolver::QPSolver(const QP& qp) :
 //***************************************************cc***************************
 VectorValues QPSolver::solveWithCurrentWorkingSet(
     const InequalityFactorGraph& workingSet) const {
-  GaussianFactorGraph workingGraph = baseGraph_;
+  GaussianFactorGraph workingGraph = qp_.cost;
+  workingGraph.push_back(qp_.equalities);
   for (const LinearInequality::shared_ptr& factor : workingSet) {
     if (factor->active())
       workingGraph.push_back(factor);
@@ -52,28 +50,23 @@ VectorValues QPSolver::solveWithCurrentWorkingSet(
 }
 
 //******************************************************************************
-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
-  std::vector < std::pair<Key, Matrix> > Aterms = collectDualJacobians
+  TermsContainer Aterms = collectDualJacobians<LinearEquality>(
-      < LinearEquality > (key, qp_.equalities, equalityVariableIndex_);
+      key, qp_.equalities, equalityVariableIndex_);
-  std::vector < std::pair<Key, Matrix> > AtermsInequalities =
+  TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
-      collectDualJacobians < LinearInequality
+      key, workingSet, inequalityVariableIndex_);
-          > (key, workingSet, inequalityVariableIndex_);
   Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
-      AtermsInequalities.end());
+                AtermsInequalities.end());
 
   // Collect the gradients of unconstrained cost factors to the b vector
   if (Aterms.size() > 0) {
-    Vector b = Vector::Zero(delta.at(key).size());
+    Vector b = qp_.costGradient(key, delta);
-    if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
+    // to compute the least-square approximation of dual variables
-      for (size_t factorIx : costVariableIndex_[key]) {
+    return boost::make_shared<JacobianFactor>(Aterms, b);
-        GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
-        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>();
   }