[unfinished] prototyping inequality SQP with Luca.

gtsam_unstable/linear/LinearInequality.h

@@ -44,12 +44,23 @@ public:
       Base(), active_(true) {
   }
 
-  /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
+  /** Conversion from HessianFactor */
   explicit LinearInequality(const HessianFactor& hf) {
     throw std::runtime_error(
         "Cannot convert HessianFactor to LinearInequality");
   }
 
+  /** Conversion from JacobianFactor */
+  explicit LinearInequality(const JacobianFactor& jf) : Base(jf), dualKey_(dualKey), active_(true) {
+    if (!jf.isConstrained()) {
+      throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality");
+    }
+
+    if (jf.get_model()->dim() != 1) {
+      throw std::runtime_error("Only support single-valued inequality factor!");
+    }
+  }
+
   /** Construct unary factor */
   LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
       Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(

gtsam_unstable/nonlinear/NonlinearConstraint.h

@@ -13,11 +13,12 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-//#include "DualKeyGenerator.h"
 
 namespace gtsam {
 
 class NonlinearConstraint {
+
+protected:
   Key dualKey_;
 
 public:

gtsam_unstable/nonlinear/NonlinearInequality.h

@@ -0,0 +1,93 @@
+/**
+ * @file 	 NonlinearConstraint.h
+ * @brief  
+ * @author Duy-Nguyen Ta
+ * @date 	 Sep 30, 2013
+ */
+
+#pragma once
+
+#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
+
+namespace gtsam {
+
+class NonlinearInequality : public NonlinearConstraint {
+  bool active_;
+
+  typedef NonlinearConstraint Base;
+
+public:
+  typedef boost::shared_ptr<NonlinearInequality> shared_ptr;
+
+public:
+  /// Construct with dual key
+  NonlinearInequality(Key dualKey) : Base(dualKey), active_(true) {}
+
+  /**
+   * compute the HessianFactor of the (-dual * constraintHessian) for the qp subproblem's objective function
+   */
+  virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x,
+      const VectorValues& duals) const = 0;
+};
+
+/* ************************************************************************* */
+/** A convenient base class for creating a nonlinear equality constraint with 1
+ * variables.  To derive from this class, implement evaluateError(). */
+template<class VALUE>
+class NonlinearInequality1: public NonlinearConstraint1<VALUE>, public NonlinearInequality {
+
+public:
+
+  // typedefs for value types pulled from keys
+  typedef VALUE X;
+
+protected:
+
+  typedef NonlinearConstraint1<VALUE> Base;
+  typedef NonlinearInequality1<VALUE> This;
+
+private:
+  static const int X1Dim = traits::dimension<VALUE>::value;
+
+public:
+
+  /**
+   * Default Constructor for I/O
+   */
+  NonlinearInequality1() {
+  }
+
+  /**
+   * Constructor
+   * @param j key of the variable
+   * @param constraintDim number of dimensions of the constraint error function
+   */
+  NonlinearInequality1(Key key, Key dualKey, size_t constraintDim = 1) :
+      Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) {
+  }
+
+  virtual ~NonlinearInequality1() {
+  }
+
+  /**
+   *  Override this method to finish implementing a binary factor.
+   *  If any of the optional Matrix reference arguments are specified, it should compute
+   *  both the function evaluation and its derivative(s) in X1 (and/or X2).
+   */
+  virtual Vector
+  evaluateError(const X&, boost::optional<Matrix&> H1 = boost::none) const {
+    return (Vector(1) << computeError(X, H1));
+  }
+
+  /**
+   *  Override this method to finish implementing a binary factor.
+   *  If any of the optional Matrix reference arguments are specified, it should compute
+   *  both the function evaluation and its derivative(s) in X1 (and/or X2).
+   */
+  virtual double
+  computeError(const X&, boost::optional<Matrix&> H1 = boost::none) const = 0;
+
+};
+// \class NonlinearConstraint1
+
+} /* namespace gtsam */

gtsam_unstable/nonlinear/tests/testSQPSimple.cpp

@@ -80,11 +80,76 @@ public:
   }
 };
 
+class NonlinearInequalityFactorGraph : public FactorGraph<NonlinearFactor> {
+
+public:
+  /// default constructor
+  NonlinearInequalityFactorGraph() {
+  }
+
+  /// linearize to a LinearEqualityFactorGraph
+  LinearInequalityFactorGraph::shared_ptr linearize(
+      const Values& linearizationPoint) const {
+    LinearInequalityFactorGraph::shared_ptr linearGraph(
+        new LinearInequalityFactorGraph());
+    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+      JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
+          factor->linearize(linearizationPoint));
+      NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
+      linearGraph->add(LinearInequality(*jacobian, constraint->dualKey()));
+    }
+    return linearGraph;
+  }
+
+  /**
+   * Return true if the error is <= 0.0
+   */
+  bool checkFeasibility(const Values& values, double tol) const {
+    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+      NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
+          factor);
+      Vector error = noiseModelFactor->unwhitenedError(values);
+      // TODO: Do we need to check if it's active or not?
+      if (error[0] > tol) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /**
+   * Return true if the max absolute error all factors is less than a tolerance
+   */
+  bool checkDualFeasibility(const VectorValues& duals, double tol) const {
+    BOOST_FOREACH(const Vector& dual, duals){
+      if (dual[0] < 0.0) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /**
+   * Return true if the max absolute error all factors is less than a tolerance
+   */
+  bool checkComplimentaryCondition(const Values& values, const VectorValues& duals, double tol) const {
+    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+      NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
+          factor);
+      Vector error = noiseModelFactor->unwhitenedError(values);
+      if (error[0] > 0.0) {
+        return false;
+      }
+    }
+    return true;
+  }
+};
 
 struct NLP {
   NonlinearFactorGraph cost;
   NonlinearEqualityFactorGraph linearEqualities;
   NonlinearEqualityFactorGraph nonlinearEqualities;
+  NonlinearInequalityFactorGraph linearInequalities;
 };
 
 struct SQPSimpleState {
@@ -117,14 +182,16 @@ public:
 
   /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0
   bool isDualFeasible(const VectorValues& delta) const {
-    return delta.vector().lpNorm<Eigen::Infinity>() < errorTol;
+    return delta.vector().lpNorm<Eigen::Infinity>() < errorTol
+        && nlp_.linearInequalities.checkDualFeasibility(errorTol);
 //    return false;
   }
 
   /// Check if c(x) == 0
   bool isPrimalFeasible(const SQPSimpleState& state) const {
     return nlp_.linearEqualities.checkFeasibility(state.values, errorTol)
-        && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol);
+        && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol)
+        && nlp_.linearInequalities.checkFeasibility(state.values, errorTol);
   }
 
   /// Check convergence
@@ -147,6 +214,8 @@ public:
     qp.equalities.add(*nlp_.linearEqualities.linearize(state.values));
     qp.equalities.add(*nlp_.nonlinearEqualities.linearize(state.values));
 
+    qp.inequalities.add(*nlp_.linearInequalities.linearize(state.values));
+
     if (debug)
       qp.print("QP subproblem:");
 
@@ -206,6 +275,7 @@ public:
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam_unstable/nonlinear/NonlinearInequality.h>
 
 using namespace std;
 using namespace gtsam::symbol_shorthand;
@@ -353,7 +423,8 @@ public:
     return (Vector(1) << pose.x()).finished();
   }
 };
-TEST_UNSAFE(testSQPSimple, poseOnALine) {
+
+TEST(testSQPSimple, poseOnALine) {
   const Key dualKey = 0;
 
 
@@ -380,6 +451,50 @@ TEST_UNSAFE(testSQPSimple, poseOnALine) {
   cout << "hessian: \n" << hessian << endl;
 }
 
+
+//******************************************************************************
+/**
+ * Inequality boundary constraint
+ *      x <= bound
+ */
+class UpperBoundX : public NonlinearInequality1<Pose3> {
+  typedef NonlinearInequality1<Pose3> Base;
+  double bound_;
+public:
+  UpperBoundX(Key key, double bound, Key dualKey) : Base(key, dualKey, 1), bound_(bound) {
+  }
+
+  double computeError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
+    if (H)
+      *H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished();
+    return pose.x() - bound_;
+  }
+};
+
+TEST(testSQPSimple, poseOnALine) {
+  const Key dualKey = 0;
+
+
+  //Instantiate NLP
+  NLP nlp;
+  nlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(-1, 0, 0)), noiseModel::Unit::Create(6)));
+  UpperBoundX constraint(X(1), 0, dualKey);
+  nlp.nonlinearInequalities.add(constraint);
+
+  Values initialValues;
+  initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(-1,0,0)));
+
+  Values expectedSolution;
+  expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3()));
+
+  // Instantiate SQP
+  SQPSimple sqpSimple(nlp);
+  Values actualSolution = sqpSimple.optimize(initialValues).first;
+
+  CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
+  actualSolution.print("actualSolution: ");
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;