diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp
index 042b62ea1..d14d8f9af 100644
--- a/gtsam_unstable/linear/LPSolver.cpp
+++ b/gtsam_unstable/linear/LPSolver.cpp
@@ -22,13 +22,56 @@ LPSolver::LPSolver(const LP &lp) :
   constrainedKeys_.merge(lp_.inequalities.keys());
 }
 
+//******************************************************************************
+GaussianFactorGraph LPSolver::buildCostFunction(const VectorValues &xk) const {
+  GaussianFactorGraph graph;
+  for (LinearCost::const_iterator it = lp_.cost.begin(); it != lp_.cost.end();
+       ++it) {
+    size_t dim = lp_.cost.getDim(it);
+    Vector b = xk.at(*it) - lp_.cost.getA(it).transpose();  // b = xk-g
+    graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
+  }
+
+  KeySet allKeys = lp_.inequalities.keys();
+  allKeys.merge(lp_.equalities.keys());
+  allKeys.merge(KeySet(lp_.cost.keys()));
+  // Add corresponding factors for all variables that are not explicitly in the
+  // cost function. Gradients of the cost function wrt to these variables are 
+  // zero (g=0), so b=xk
+  if (lp_.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 = lp_.constrainedKeyDimMap().at(k);
+      graph.push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
+    }
+  }
+  return graph;
+}
+
+//******************************************************************************
+GaussianFactorGraph LPSolver::buildWorkingGraph(
+    const InequalityFactorGraph &workingSet, const VectorValues &xk) const {
+  GaussianFactorGraph workingGraph;
+  // || X - Xk + g ||^2
+  workingGraph.push_back(buildCostFunction(xk));
+  workingGraph.push_back(lp_.equalities);
+  for (const LinearInequality::shared_ptr &factor : workingSet) {
+    if (factor->active()) workingGraph.push_back(factor);
+  }
+  return workingGraph;
+}
+
 //******************************************************************************
 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
   // to find the direction to move
-  VectorValues newValues = solveWithCurrentWorkingSet(state.values,
-      state.workingSet);
+  GaussianFactorGraph workingGraph =
+      buildWorkingGraph(state.workingSet, state.values);
+  VectorValues newValues = workingGraph.optimize();
+
   // If we CAN'T move further
   // LP: projection on the constraints' nullspace is zero: we are at a vertex
   if (newValues.equals(state.values, 1e-7)) {
@@ -80,48 +123,6 @@ 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());
-  for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) {
-    size_t dim = cost.getDim(it);
-    Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g
-    graph->push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
-  }
-
-  KeySet allKeys = lp_.inequalities.keys();
-  allKeys.merge(lp_.equalities.keys());
-  allKeys.merge(KeySet(lp_.cost.keys()));
-  // Add corresponding factors for all variables that are not explicitly in the
-  // 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 = lp_.constrainedKeyDimMap().at(k);
-      graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
-    }
-  }
-  return graph;
-}
-
-//******************************************************************************
-VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
-    const InequalityFactorGraph &workingSet) const {
-  GaussianFactorGraph workingGraph;
-  // || X - Xk + g ||^2
-  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);
-  }
-  return workingGraph.optimize();
-}
-
 //******************************************************************************
 boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
     Key key, const InequalityFactorGraph &workingSet,
diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h
index bf081c05e..915713af4 100644
--- a/gtsam_unstable/linear/LPSolver.h
+++ b/gtsam_unstable/linear/LPSolver.h
@@ -49,12 +49,10 @@ public:
    * The least-square solution of this quadratic subject to a set of linear constraints
    * is the projection of the gradient onto the constraints' subspace
    */
-  GaussianFactorGraph::shared_ptr createLeastSquareFactors(
-      const LinearCost &cost, const VectorValues &xk) const;
+  GaussianFactorGraph buildCostFunction(const VectorValues &xk) const;
 
-  /// Find solution with the current working set
-  VectorValues solveWithCurrentWorkingSet(const VectorValues &xk,
-      const InequalityFactorGraph &workingSet) const;
+  GaussianFactorGraph buildWorkingGraph(
+    const InequalityFactorGraph& workingSet, const VectorValues& xk) const;
 
   /*
    * A dual factor takes the objective function and a set of constraints.
diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp
index a8ef028e8..703ce2faa 100644
--- a/gtsam_unstable/linear/QPSolver.cpp
+++ b/gtsam_unstable/linear/QPSolver.cpp
@@ -37,16 +37,16 @@ QPSolver::QPSolver(const QP& qp) :
   constrainedKeys_.merge(qp_.inequalities.keys());
 }
 
-//***************************************************cc***************************
-VectorValues QPSolver::solveWithCurrentWorkingSet(
-    const InequalityFactorGraph& workingSet) const {
-  GaussianFactorGraph workingGraph = qp_.cost;
+//******************************************************************************
+GaussianFactorGraph QPSolver::buildWorkingGraph(
+    const InequalityFactorGraph& workingSet, const VectorValues& xk) const {
+  GaussianFactorGraph workingGraph;
+  workingGraph.push_back(buildCostFunction(xk));
   workingGraph.push_back(qp_.equalities);
   for (const LinearInequality::shared_ptr& factor : workingSet) {
-    if (factor->active())
-      workingGraph.push_back(factor);
+    if (factor->active()) workingGraph.push_back(factor);
   }
-  return workingGraph.optimize();
+  return workingGraph;
 }
 
 //******************************************************************************
@@ -84,7 +84,9 @@ QPState QPSolver::iterate(const QPState& state) const {
   // Algorithm 16.3 from Nocedal06book.
   // Solve with the current working set eqn 16.39, but instead of solving for p
   // solve for x
-  VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet);
+  GaussianFactorGraph workingGraph =
+      buildWorkingGraph(state.workingSet, state.values);
+  VectorValues newValues = workingGraph.optimize();
   // If we CAN'T move further
   // if p_k = 0 is the original condition, modified by Duy to say that the state
   // update is zero.
diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h
index bd1bb2a17..9c67b9985 100644
--- a/gtsam_unstable/linear/QPSolver.h
+++ b/gtsam_unstable/linear/QPSolver.h
@@ -43,9 +43,14 @@ public:
   /// Constructor
   QPSolver(const QP& qp);
 
-  /// Find solution with the current working set
-  VectorValues solveWithCurrentWorkingSet(
-      const InequalityFactorGraph& workingSet) const;
+  const GaussianFactorGraph& buildCostFunction(
+      const VectorValues& xk = VectorValues()) const {
+    return qp_.cost;
+  }
+
+  GaussianFactorGraph buildWorkingGraph(
+      const InequalityFactorGraph& workingSet,
+      const VectorValues& xk = VectorValues()) const;
 
   /// Create a dual factor
   JacobianFactor::shared_ptr createDualFactor(Key key,
diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp
index be1759428..c50b5f221 100644
--- a/gtsam_unstable/linear/tests/testLPSolver.cpp
+++ b/gtsam_unstable/linear/tests/testLPSolver.cpp
@@ -184,8 +184,6 @@ LPSolver lpSolver(lp);
 VectorValues init;
 init.insert(1, Vector::Zero(2));
 
-VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
-    InequalityFactorGraph());
 VectorValues expected_x1;
 expected_x1.insert(1, Vector::Ones(2));
 CHECK(assert_equal(expected_x1, x1, 1e-10));
@@ -195,6 +193,8 @@ boost::tie(result, duals) = lpSolver.optimize(init);
 VectorValues expectedResult;
 expectedResult.insert(1, Vector2(8./3., 2./3.));
 CHECK(assert_equal(expectedResult, result, 1e-10));
+  VectorValues x1 =
+      lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize();
 }
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp
index 1f21a9e49..e707ac911 100644
--- a/gtsam_unstable/linear/tests/testQPSolver.cpp
+++ b/gtsam_unstable/linear/tests/testQPSolver.cpp
@@ -144,7 +144,7 @@ TEST(QPSolver, indentifyActiveConstraints) {
   CHECK(workingSet.at(2)->active());// active
   CHECK(!workingSet.at(3)->active());// inactive
 
-  VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
+  VectorValues solution = solver.buildWorkingGraph(workingSet).optimize();
   VectorValues expectedSolution;
   expectedSolution.insert(X(1), kZero);
   expectedSolution.insert(X(2), kZero);