Added warmStart flag.

gtsam_unstable/linear/QPSolver.cpp

@@ -217,15 +217,15 @@ QPState QPSolver::iterate(const QPState& state) const {
 //******************************************************************************
 LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
     const LinearInequalityFactorGraph& inequalities,
-    const VectorValues& initialValues, const VectorValues& duals) const {
+    const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
   LinearInequalityFactorGraph workingSet;
   BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
     LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-    if (duals.exists(workingFactor->dualKey())) {
+    if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
       workingFactor->activate();
     }
     else {
-      if (duals.size() > 0) {
+      if (useWarmStart == true && duals.size() > 0) {
         workingFactor->inactivate();
       } else {
         double error = workingFactor->error(initialValues);
@@ -244,11 +244,11 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
 
 //******************************************************************************
 pair<VectorValues, VectorValues> QPSolver::optimize(
-    const VectorValues& initialValues, const VectorValues& duals) const {
+    const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
 
   // Initialize workingSet from the feasible initialValues
   LinearInequalityFactorGraph workingSet =
-      identifyActiveConstraints(qp_.inequalities, initialValues, duals);
+      identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart);
   QPState state(initialValues, duals, workingSet, false, 0);
 
   /// main loop of the solver

gtsam_unstable/linear/QPSolver.h

@@ -185,7 +185,7 @@ public:
   LinearInequalityFactorGraph identifyActiveConstraints(
       const LinearInequalityFactorGraph& inequalities,
       const VectorValues& initialValues,
-      const VectorValues& duals = VectorValues()) const;
+      const VectorValues& duals = VectorValues(), bool useWarmStart = true) const;
 
   /** Optimize with a provided initial values
    * For this version, it is the responsibility of the caller to provide
@@ -193,7 +193,7 @@ public:
    * @return a pair of <primal, dual> solutions
    */
   std::pair<VectorValues, VectorValues> optimize(
-      const VectorValues& initialValues, const VectorValues& duals = VectorValues()) const;
+      const VectorValues& initialValues, const VectorValues& duals = VectorValues(), bool useWarmStart = true) const;
 
 };
 

gtsam_unstable/nonlinear/LCNLPSolver.cpp

@@ -70,13 +70,13 @@ VectorValues LCNLPSolver::initializeDuals() const {
 }
 
 /* ************************************************************************* */
-std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValues, bool debug) const {
+std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValues, bool useWarmStart, bool debug) const {
   LCNLPState state(initialValues);
   state.duals = initializeDuals();
   while (!state.converged && state.iterations < 100) {
     if (debug)
     std::cout << "state: iteration " << state.iterations << std::endl;
-    state = iterate(state, debug);
+    state = iterate(state, useWarmStart, debug);
   }
   if (debug)
   std::cout << "Number of iterations: " << state.iterations << std::endl;
@@ -84,7 +84,7 @@ std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValue
 }
 
 /* ************************************************************************* */
-LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool debug) const {
+LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool debug) const {
 
   // construct the qp subproblem
   QP qp;
@@ -98,14 +98,14 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool debug) const {
   // solve the QP subproblem
   VectorValues delta, duals;
   QPSolver qpSolver(qp);
-  if (state.iterations == 0)
+  if (useWarmStart == false || 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);
+    boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, useWarmStart);
   }
 
   if (debug)

gtsam_unstable/nonlinear/LCNLPSolver.h

@@ -104,13 +104,13 @@ public:
   /**
    * Single iteration of SQP
    */
-  LCNLPState iterate(const LCNLPState& state, bool debug = false) const;
+  LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const;
 
   VectorValues initializeDuals() const;
   /**
    * Main optimization function. new
    */
-  std::pair<Values, VectorValues> optimize(const Values& initialValues, bool debug = false) const;
+  std::pair<Values, VectorValues> optimize(const Values& initialValues, bool useWarmStart = true, bool debug = false) const;
 
 };
 }

gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp

@@ -348,7 +348,8 @@ TEST(testlcnlpSolver, posesInA2DBox) {
 
   // Instantiate LCNLPSolver
   LCNLPSolver lcnlpSolver(lcnlp);
-  Values actualSolution = lcnlpSolver.optimize(initialValues).first;
+  bool useWarmStart = true;
+  Values actualSolution = lcnlpSolver.optimize(initialValues, useWarmStart).first;
 
 //  cout << "Rotation angles: " << endl;
 //  for (size_t i = 1; i<=3; i++) {
@@ -367,6 +368,7 @@ TEST(testlcnlpSolver, posesInA2DBox) {
 
 //******************************************************************************
 int main() {
+  cout<<"here: "<<endl;
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }