Added warm start for initializing active set.

gtsam_unstable/linear/QPSolver.cpp

@@ -181,13 +181,13 @@ QPState QPSolver::iterate(const QPState& state) const {
 
     // If all inequality constraints are satisfied: We have the solution!!
     if (leavingFactor < 0) {
-      return QPState(newValues, duals, state.workingSet, true);
+      return QPState(newValues, duals, state.workingSet, true, state.iterations+1);
     }
     else {
       // Inactivate the leaving constraint
       LinearInequalityFactorGraph newWorkingSet = state.workingSet;
       newWorkingSet.at(leavingFactor)->inactivate();
-      return QPState(newValues, duals, newWorkingSet, false);
+      return QPState(newValues, duals, newWorkingSet, false, state.iterations+1);
     }
   }
   else {
@@ -210,23 +210,33 @@ QPState QPSolver::iterate(const QPState& state) const {
     // step!
     newValues = state.values + alpha * p;
 
-    return QPState(newValues, state.duals, newWorkingSet, false);
+    return QPState(newValues, state.duals, newWorkingSet, false, state.iterations+1);
   }
 }
 
 //******************************************************************************
 LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
     const LinearInequalityFactorGraph& inequalities,
-    const VectorValues& initialValues) const {
+    const VectorValues& initialValues, const VectorValues& duals) const {
   LinearInequalityFactorGraph workingSet;
-  BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
+  BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
     LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-    double error = workingFactor->error(initialValues);
+    if (duals.exists(workingFactor->dualKey())) {
-    if (fabs(error)>1e-7){
-      workingFactor->inactivate();
-    } else {
       workingFactor->activate();
     }
+    else {
+      if (duals.size() > 0) {
+        workingFactor->inactivate();
+      } else {
+        double error = workingFactor->error(initialValues);
+        if (fabs(error)<1e-7) {
+          workingFactor->activate();
+        }
+        else {
+          workingFactor->inactivate();
+        }
+      }
+    }
     workingSet.push_back(workingFactor);
   }
   return workingSet;
@@ -234,18 +244,18 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
 
 //******************************************************************************
 pair<VectorValues, VectorValues> QPSolver::optimize(
-    const VectorValues& initialValues) const {
+    const VectorValues& initialValues, const VectorValues& duals) const {
 
   // Initialize workingSet from the feasible initialValues
   LinearInequalityFactorGraph workingSet =
-      identifyActiveConstraints(qp_.inequalities, initialValues);
+      identifyActiveConstraints(qp_.inequalities, initialValues, duals);
-  QPState state(initialValues, VectorValues(), workingSet, false);
+  QPState state(initialValues, duals, workingSet, false, 0);
 
   /// main loop of the solver
   while (!state.converged) {
     state = iterate(state);
   }
-
+  std::cout << "Number of inner iterations: " << state.iterations << std::endl;
   return make_pair(state.values, state.duals);
 }
 

gtsam_unstable/linear/QPSolver.h

@@ -31,17 +31,18 @@ struct QPState {
   VectorValues duals;
   LinearInequalityFactorGraph workingSet;
   bool converged;
+  size_t iterations;
 
   /// default constructor
   QPState() :
-      values(), duals(), workingSet(), converged(false) {
+      values(), duals(), workingSet(), converged(false), iterations(0) {
   }
 
   /// constructor with initial values
   QPState(const VectorValues& initialValues, const VectorValues& initialDuals,
-      const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) :
+      const LinearInequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) :
       values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
-          _converged) {
+          _converged), iterations(_iterations) {
   }
 };
 
@@ -183,7 +184,8 @@ public:
    */
   LinearInequalityFactorGraph identifyActiveConstraints(
       const LinearInequalityFactorGraph& inequalities,
-      const VectorValues& initialValues) const;
+      const VectorValues& initialValues,
+      const VectorValues& duals = VectorValues()) const;
 
   /** Optimize with a provided initial values
    * For this version, it is the responsibility of the caller to provide
@@ -191,7 +193,7 @@ public:
    * @return a pair of <primal, dual> solutions
    */
   std::pair<VectorValues, VectorValues> optimize(
-      const VectorValues& initialValues) const;
+      const VectorValues& initialValues, const VectorValues& duals = VectorValues()) const;
 
 };
 

gtsam_unstable/nonlinear/LCNLPSolver.cpp

@@ -19,6 +19,7 @@
 
 #include <gtsam_unstable/nonlinear/LCNLPSolver.h>
 #include <gtsam_unstable/linear/QPSolver.h>
+#include <iostream>
 
 namespace gtsam {
 
@@ -75,12 +76,13 @@ std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValue
   while (!state.converged && state.iterations < 100) {
     state = iterate(state);
   }
+  std::cout << "Number of iterations: " << state.iterations << std::endl;
   return std::make_pair(state.values, state.duals);
 }
 
 /* ************************************************************************* */
 LCNLPState LCNLPSolver::iterate(const LCNLPState& state) const {
-  static const bool debug = false;
+  static const bool debug = true;
 
   // construct the qp subproblem
   QP qp;
@@ -88,19 +90,29 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state) const {
   qp.equalities.add(*lcnlp_.linearEqualities.linearize(state.values));
   qp.inequalities.add(*lcnlp_.linearInequalities.linearize(state.values));
 
-  if (debug)
+//  if (debug)
-    qp.print("QP subproblem:");
+//    qp.print("QP subproblem:");
 
   // solve the QP subproblem
   VectorValues delta, duals;
   QPSolver qpSolver(qp);
-  boost::tie(delta, duals) = qpSolver.optimize();
+  if (state.iterations == 0)
+    boost::tie(delta, duals) = qpSolver.optimize();
+  else {
+    VectorValues zeroInitialValues;
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) {
+      zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
+    }
+    boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals);
+  }
 
+  if (debug)
+    state.values.print("state.values: ");
   if (debug)
     delta.print("delta = ");
 
-  if (debug)
+//  if (debug)
-    duals.print("duals = ");
+//    duals.print("duals = ");
 
   // update new state
   LCNLPState newState;
@@ -109,6 +121,9 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state) const {
   newState.converged = checkConvergence(newState, delta);
   newState.iterations = state.iterations + 1;
 
+  if (debug)
+    newState.print("newState: ");
+
   return newState;
 }
 }