[REFACTOR] Move identifyActiveConstraints from QP and LP to the ActiveSetSolver.

gtsam_unstable/linear/ActiveSetSolver.cpp

@@ -7,6 +7,7 @@
  */
 
 #include <gtsam_unstable/linear/ActiveSetSolver.h>
+#include "InfeasibleInitialValues.h"
 
 namespace gtsam {
 
@@ -129,4 +130,33 @@ boost::tuple<double, int> ActiveSetSolver::computeStepSize(
   return boost::make_tuple(minAlpha, closestFactorIx);
 }
 
+//******************************************************************************
+InequalityFactorGraph ActiveSetSolver::identifyActiveConstraints(
+    const InequalityFactorGraph& inequalities,
+    const VectorValues& initialValues, const VectorValues& duals,
+    bool useWarmStart) const {
+  InequalityFactorGraph workingSet;
+  for (const LinearInequality::shared_ptr &factor : inequalities) {
+    LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
+    if (useWarmStart && duals.size() > 0) {
+      if (duals.exists(workingFactor->dualKey()))
+        workingFactor->activate();
+      else
+        workingFactor->inactivate();
+    } else {
+      double error = workingFactor->error(initialValues);
+      // Safety guard. This should not happen unless users provide a bad init
+      if (error > 0)
+        throw InfeasibleInitialValues();
+      if (fabs(error) < 1e-7)
+        workingFactor->activate();
+      else
+        workingFactor->inactivate();
+    }
+    workingSet.push_back(workingFactor);
+  }
+  return workingSet;
+}
+  
+  
 }

gtsam_unstable/linear/ActiveSetSolver.h

@@ -64,12 +64,22 @@ public:
    */
   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;
+  /*
+   * Given an initial value this function determine which constraints are active
+   * which can be used to initialize the working set.
+   * A constraint Ax <= b  is active if we have an x' s.t. Ax' = b
+   */
+  /// Identify active constraints based on initial values.
+  InequalityFactorGraph identifyActiveConstraints(
+      const InequalityFactorGraph& inequalities,
+      const VectorValues& initialValues, const VectorValues& duals =
+          VectorValues(), bool useWarmStart = true) const;
 
 protected:
   /**

gtsam_unstable/linear/LPSolver.cpp

@@ -145,29 +145,6 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
   }
 }
 
-//******************************************************************************
-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));
-    double error = workingFactor->error(initialValues);
-    // TODO: find a feasible initial point for LPSolver.
-    // For now, we just throw an exception
-    if (error > 0)
-      throw InfeasibleInitialValues();
-
-    if (fabs(error) < 1e-7) {
-      workingFactor->activate();
-    } else {
-      workingFactor->inactivate();
-    }
-    workingSet.push_back(workingFactor);
-  }
-  return workingSet;
-}
-
 //******************************************************************************
 std::pair<VectorValues, VectorValues> LPSolver::optimize(
     const VectorValues &initialValues, const VectorValues &duals) const {

gtsam_unstable/linear/LPSolver.h

@@ -34,7 +34,7 @@ public:
    * LP problems. At the end of this iteration the problem should either be found
    * to be unfeasible, solved or the current state changed to reflect a new
    * working set.
-   */
+   */ //SAME
   LPState iterate(const LPState &state) const;
 
   /**
@@ -53,6 +53,7 @@ public:
       const LinearCost &cost, const VectorValues &xk) const;
 
   /// Find solution with the current working set
+  //SAME
   VectorValues solveWithCurrentWorkingSet(const VectorValues &xk,
       const InequalityFactorGraph &workingSet) const;
 
@@ -62,23 +63,16 @@ public:
    * for the following problem: f' = - lambda * g' where f is the objection
    * function g are dual factors and lambda is the lagrangian multiplier.
    */
+  //SAME
   JacobianFactor::shared_ptr createDualFactor(Key key,
       const InequalityFactorGraph &workingSet, const VectorValues &delta) const;
 
   /// TODO(comment)
+  //SAME
   boost::tuple<double, int> computeStepSize(
       const InequalityFactorGraph &workingSet, const VectorValues &xk,
       const VectorValues &p) const;
 
-  /*
-   * Given an initial value this function determine which constraints are active
-   * which can be used to initialize the working set.
-   * A constraint Ax <= b  is active if we have an x' s.t. Ax' = b
-   */
-  InequalityFactorGraph identifyActiveConstraints(
-      const InequalityFactorGraph &inequalities,
-      const VectorValues &initialValues, const VectorValues &duals) const;
-
   /** Optimize with the provided feasible initial values
    * TODO: throw exception if the initial values is not feasible wrt inequality constraints
    * TODO: comment duals

gtsam_unstable/linear/QPSolver.cpp

@@ -124,31 +124,6 @@ QPState QPSolver::iterate(const QPState& state) const {
   }
 }
 
-//******************************************************************************
-InequalityFactorGraph QPSolver::identifyActiveConstraints(
-    const InequalityFactorGraph& inequalities,
-    const VectorValues& initialValues, const VectorValues& duals,
-    bool useWarmStart) const {
-  InequalityFactorGraph workingSet;
-  for (const LinearInequality::shared_ptr& factor : inequalities) {
-    LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-    if (useWarmStart && duals.size() > 0) {
-      if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
-      else workingFactor->inactivate();
-    } else {
-      double error = workingFactor->error(initialValues);
-      // Safety guard. This should not happen unless users provide a bad init
-      if (error > 0) throw InfeasibleInitialValues();
-      if (fabs(error) < 1e-7)
-        workingFactor->activate();
-      else
-        workingFactor->inactivate();
-    }
-    workingSet.push_back(workingFactor);
-  }
-  return workingSet;
-}
-
 //******************************************************************************
 pair<VectorValues, VectorValues> QPSolver::optimize(
     const VectorValues& initialValues, const VectorValues& duals,

gtsam_unstable/linear/QPSolver.h

@@ -44,10 +44,12 @@ public:
   QPSolver(const QP& qp);
 
   /// Find solution with the current working set
+  //SAME
   VectorValues solveWithCurrentWorkingSet(
       const InequalityFactorGraph& workingSet) const;
 
   /// Create a dual factor
+  //SAME
   JacobianFactor::shared_ptr createDualFactor(Key key,
       const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
 
@@ -66,6 +68,7 @@ public:
    *
    * We want the minimum of all those alphas among all inactive inequality.
    */
+  //SAME
   boost::tuple<double, int> computeStepSize(
       const InequalityFactorGraph& workingSet, const VectorValues& xk,
       const VectorValues& p) const;
@@ -73,12 +76,6 @@ public:
   /// Iterate 1 step, return a new state with a new workingSet and values
   QPState iterate(const QPState& state) const;
 
-  /// Identify active constraints based on initial values.
-  InequalityFactorGraph identifyActiveConstraints(
-      const InequalityFactorGraph& inequalities,
-      const VectorValues& initialValues, const VectorValues& duals =
-          VectorValues(), bool useWarmStart = true) const;
-
   /**
    * Optimize with provided initial values
    * For this version, it is the responsibility of the caller to provide