factor out QPInitSolver

gtsam_unstable/linear/QPInitSolver.h

@@ -0,0 +1,45 @@
+/**
+ * @file     QPInitSolver.h
+ * @brief    This QPInitSolver implements the strategy in Matlab.
+ * @author   Duy Nguyen Ta
+ * @author   Ivan Dario Jimenez
+ * @date     6/16/16
+ */
+
+#pragma once
+
+#include <gtsam_unstable/linear/LPInitSolver.h>
+
+namespace gtsam {
+
+/**
+ * This class finds a feasible solution for a QP problem.
+ * This uses the Matlab strategy for initialization
+ * For details, see
+ * http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22
+ */
+class QPInitSolver {
+    const QP& qp_;
+public:
+    /// Constructor with a QP problem
+    QPInitSolver(const QP& qp) : qp_(qp) {}
+
+    /**
+     * @return a feasible initialization point
+     */
+    VectorValues solve() 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
+      Key newKey = maxKey(qp_) + 1;
+      initProblem.cost = LinearCost(newKey, Vector::Ones(1));
+      initProblem.equalities = qp_.equalities;
+      initProblem.inequalities = qp_.inequalities;
+      LPInitSolver initSolver(initProblem);
+      return initSolver.solve();
+    }
+};
+
+
+}

gtsam_unstable/linear/QPSolver.cpp

@@ -21,8 +21,8 @@
 #include <gtsam_unstable/linear/QPSolver.h>
 #include <gtsam_unstable/linear/LPSolver.h>
 #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
+#include <gtsam_unstable/linear/QPInitSolver.h>
 #include <boost/range/adaptor/map.hpp>
-#include <gtsam_unstable/linear/LPInitSolver.h>
 
 using namespace std;
 
@@ -169,16 +169,8 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
 
 //******************************************************************************
 pair<VectorValues, VectorValues> QPSolver::optimize() const {
-  //Make an LP with any linear cost function. It doesn't matter for initialization.
+  QPInitSolver initSolver(qp_);
-  LP initProblem;
-  // make an unrelated key for a random variable cost
-  Key newKey = maxKey(qp_) + 1;
-  initProblem.cost = LinearCost(newKey, Vector::Ones(1));
-  initProblem.equalities = qp_.equalities;
-  initProblem.inequalities = qp_.inequalities;
-  LPInitSolver initSolver(initProblem);
   VectorValues initValues = initSolver.solve();
-
   return optimize(initValues);
 }
 

gtsam_unstable/linear/QPSolver.h

@@ -96,9 +96,6 @@ public:
 
   /**
    * For this version the caller will not have to provide an initial value
-   * Uses the matlab strategy for initialization
-   * See  http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22
-   * For details
    * @return a pair of <primal, dual> solutions
    */
   std::pair<VectorValues, VectorValues> optimize() const;