[REFACTOR] Move ComputeStepSize from subclasses to ActiveSetSolver

gtsam_unstable/linear/ActiveSetSolver.cpp

@@ -89,7 +89,6 @@ GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph(
   }
   return dualGraph;
 }
-
 /*
  * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
  *
@@ -100,8 +99,8 @@ GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph(
  */
 boost::tuple<double, int> ActiveSetSolver::computeStepSize(
     const InequalityFactorGraph& workingSet, const VectorValues& xk,
-    const VectorValues& p, const double& startAlpha) const {
-  double minAlpha = startAlpha;
+    const VectorValues& p) const {
+  double minAlpha = startAlpha_;
   int closestFactorIx = -1;
   for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
     const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);

gtsam_unstable/linear/ActiveSetSolver.h

@@ -23,7 +23,7 @@ protected:
   KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
   VariableIndex equalityVariableIndex_,
       inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
-
+  double startAlpha_;
 public:
   typedef std::vector<std::pair<Key, Matrix> > TermsContainer; //!< vector of key matrix pairs
   //Matrices are usually the A term for a factor.
@@ -86,8 +86,8 @@ protected:
    * Protected constructor because this class doesn't have any meaning without
    * a concrete Programming problem to solve.
    */
-  ActiveSetSolver() :
-      constrainedKeys_() {
+  ActiveSetSolver(double startAlpha) :
+      constrainedKeys_(), startAlpha_(startAlpha) {
   }
 
   /**
@@ -97,7 +97,7 @@ protected:
    */
   boost::tuple<double, int> computeStepSize(
       const InequalityFactorGraph& workingSet, const VectorValues& xk,
-      const VectorValues& p, const double& startAlpha) const;
+      const VectorValues& p) const;
 };
 
 /**

gtsam_unstable/linear/LPSolver.cpp

@@ -14,7 +14,7 @@
 namespace gtsam {
 //******************************************************************************
 LPSolver::LPSolver(const LP &lp) :
-    lp_(lp) {
+    ActiveSetSolver(std::numeric_limits<double>::infinity()), lp_(lp) {
   // Variable index
   equalityVariableIndex_ = VariableIndex(lp_.equalities);
   inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
@@ -162,14 +162,6 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
   }
 }
 
-//******************************************************************************
-boost::tuples::tuple<double, int> LPSolver::computeStepSize(
-    const InequalityFactorGraph &workingSet, const VectorValues &xk,
-    const VectorValues &p) const {
-  return ActiveSetSolver::computeStepSize(workingSet, xk, p,
-      std::numeric_limits<double>::infinity());
-}
-
 //******************************************************************************
 pair<VectorValues, VectorValues> LPSolver::optimize() const {
   LPInitSolver initSolver(lp_);

gtsam_unstable/linear/LPSolver.h

@@ -67,12 +67,6 @@ public:
   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;
-
   /** 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

@@ -30,7 +30,7 @@ namespace gtsam {
 
 //******************************************************************************
 QPSolver::QPSolver(const QP& qp) :
-    qp_(qp) {
+    ActiveSetSolver(1.0), qp_(qp) {
   equalityVariableIndex_ = VariableIndex(qp_.equalities);
   inequalityVariableIndex_ = VariableIndex(qp_.inequalities);
   constrainedKeys_ = qp_.equalities.keys();
@@ -71,14 +71,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(
     return boost::make_shared<JacobianFactor>();
   }
 }
-
-//******************************************************************************
-boost::tuple<double, int> QPSolver::computeStepSize(
-    const InequalityFactorGraph& workingSet, const VectorValues& xk,
-    const VectorValues& p) const {
-  return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1);
-}
-
+  
 //******************************************************************************
 QPState QPSolver::iterate(const QPState& state) const {
   // Algorithm 16.3 from Nocedal06book.

gtsam_unstable/linear/QPSolver.h

@@ -68,10 +68,6 @@ 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;
 
   /// Iterate 1 step, return a new state with a new workingSet and values
   QPState iterate(const QPState& state) const;