support params and verbosity debug info for LinearConstraintSQP

gtsam_unstable/nonlinear/LinearConstraintSQP.cpp

@@ -25,12 +25,12 @@ namespace gtsam {
 
 /* ************************************************************************* */
 bool LinearConstraintSQP::isStationary(const VectorValues& delta) const {
-  return delta.vector().lpNorm<Eigen::Infinity>() < errorTol;
+  return delta.vector().lpNorm<Eigen::Infinity>() < params_.errorTol;
 }
 
 /* ************************************************************************* */
 bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state) const {
-  return lcnlp_.linearEqualities.checkFeasibility(state.values, errorTol);
+  return lcnlp_.linearEqualities.checkFeasibility(state.values, params_.errorTol);
 }
 
 /* ************************************************************************* */
@@ -51,7 +51,7 @@ bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
 /* ************************************************************************* */
 bool LinearConstraintSQP::isComplementary(const LinearConstraintNLPState& state) const {
   return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary(
-      state.values, state.duals, errorTol);
+      state.values, state.duals, params_.errorTol);
 }
 
 /* ************************************************************************* */
@@ -74,8 +74,8 @@ VectorValues LinearConstraintSQP::initializeDuals() const {
 }
 
 /* ************************************************************************* */
-LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPState& state, bool useWarmStart,
+LinearConstraintNLPState LinearConstraintSQP::iterate(
-    bool debug) const {
+    const LinearConstraintNLPState& state) const {
 
   // construct the qp subproblem
   QP qp;
@@ -83,8 +83,8 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPS
   qp.equalities.add(*lcnlp_.linearEqualities.linearize(state.values));
   qp.inequalities.add(*lcnlp_.linearInequalities.linearize(state.values));
 
-//  if (debug)
+  if(params_.verbosity >= NonlinearOptimizerParams::LINEAR)
-//    qp.print("QP subproblem:");
+    qp.print("QP subproblem:");
 
   // solve the QP subproblem
   VectorValues delta, duals;
@@ -94,15 +94,10 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPS
     zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
   }
   boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
-      useWarmStart);
+      params_.warmStart);
 
-  if (debug)
+  if(params_.verbosity >= NonlinearOptimizerParams::DELTA)
-    state.values.print("state.values: ");
+    delta.print("Delta");
-  if (debug)
-    delta.print("delta = ");
-
-//  if (debug)
-//    duals.print("duals = ");
 
   // update new state
   LinearConstraintNLPState newState;
@@ -111,23 +106,25 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPS
   newState.converged = checkConvergence(newState, delta);
   newState.iterations = state.iterations + 1;
 
-  if (debug)
+  if(params_.verbosity >= NonlinearOptimizerParams::VALUES) {
-    newState.print("newState: ");
+    newState.values.print("Values");
+    newState.duals.print("Duals");
+  }
 
   return newState;
 }
 
 /* ************************************************************************* */
 std::pair<Values, VectorValues> LinearConstraintSQP::optimize(
-    const Values& initialValues, bool useWarmStart, bool debug) const {
+    const Values& initialValues) const {
   LinearConstraintNLPState state(initialValues);
   state.duals = initializeDuals();
-  while (!state.converged && state.iterations < 100) {
+  while (!state.converged && state.iterations < params_.maxIterations) {
-    if (debug)
+    if(params_.verbosity >= NonlinearOptimizerParams::ERROR)
-      std::cout << "state: iteration " << state.iterations << std::endl;
+      std::cout << "Iteration # " << state.iterations << std::endl;
-    state = iterate(state, useWarmStart, debug);
+    state = iterate(state);
   }
-  if (debug)
+  if(params_.verbosity >= NonlinearOptimizerParams::TERMINATION)
     std::cout << "Number of iterations: " << state.iterations << std::endl;
   return std::make_pair(state.values, state.duals);
 }

gtsam_unstable/nonlinear/LinearConstraintSQP.h

@@ -19,6 +19,7 @@
 
 #pragma once
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
 #include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h>
 #include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
 
@@ -62,16 +63,32 @@ struct LinearConstraintNLPState {
   }
 };
 
+/** Parameters for Gauss-Newton optimization, inherits from
+ * NonlinearOptimizationParams.
+ */
+class GTSAM_EXPORT LinearConstraintSQPParams : public NonlinearOptimizerParams {
+public:
+  bool warmStart;
+
+  LinearConstraintSQPParams() : NonlinearOptimizerParams(), warmStart(false) {}
+
+  void setWarmStart(bool _warmStart) {
+    _warmStart = warmStart;
+  }
+};
+
 /**
  * Simple SQP optimizer to solve nonlinear constrained problems
  * ONLY linear constraints are supported.
  */
 class LinearConstraintSQP {
   LinearConstraintNLP lcnlp_;
-  static const double errorTol = 1e-5;
+  LinearConstraintSQPParams params_;
+
 public:
-  LinearConstraintSQP(const LinearConstraintNLP& lcnlp) :
+  LinearConstraintSQP(const LinearConstraintNLP& lcnlp,
-      lcnlp_(lcnlp) {
+      const LinearConstraintSQPParams& params = LinearConstraintSQPParams()) :
+        lcnlp_(lcnlp), params_(params) {
   }
 
   /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0
@@ -104,7 +121,7 @@ public:
   /**
    * Single iteration of SQP
    */
-  LinearConstraintNLPState iterate(const LinearConstraintNLPState& state, bool useWarmStart = true, bool debug = false) const;
+  LinearConstraintNLPState iterate(const LinearConstraintNLPState& state) const;
 
   /// Intialize all dual variables to zeros
   VectorValues initializeDuals() const;
@@ -112,7 +129,7 @@ public:
   /**
    * Main optimization function. new
    */
-  std::pair<Values, VectorValues> optimize(const Values& initialValues, bool useWarmStart = true, bool debug = false) const;
+  std::pair<Values, VectorValues> optimize(const Values& initialValues) const;
 
 };
 }

gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp

@@ -207,7 +207,6 @@ TEST(testlcnlpSolver, inequalityConstraint) {
 
 //******************************************************************************
 int main() {
-  cout<<"here: "<<endl;
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }