[EXPERIMENTAL] Initial SQP Solver. DO NOT BUILD.

gtsam_unstable/linear/InfeasibleInitialValues.h

@@ -10,7 +10,7 @@
 namespace gtsam {
 /* ************************************************************************* */
 /** An exception indicating that the provided initial value is infeasible
- * Also used to indicatethat the noise model dimension passed into a
+ * Also used to inzdicatethat the noise model dimension passed into a
  * JacobianFactor has a different dimensionality than the factor. */
 class InfeasibleInitialValues: public ThreadsafeException<
     InfeasibleInitialValues> {

gtsam_unstable/linear/NP.h

@@ -0,0 +1,37 @@
+/**
+ * @file     NP.h
+ * @brief    Use this virtual class to represent a non-linear
+ *           problem. An implementation of this should provide
+ *           approximation and differentiation services to
+ *           allow SQP to work on the given problem.
+ * @author   Ivan Dario Jimenez
+ * @date     2/2/16
+ */
+
+#pragma once
+
+#include <gtsam_unstable/linear/QP.h>
+
+namespace gtsam {
+
+class NP {
+public:
+  /*
+   * This function takes the a point and returns a quadratic 
+   * problem approximation to the non-linear Problem.
+   */
+  virtual QP approximate(const Vector& x) const = 0;
+
+  /*
+   * Differentiates the objective function of the non-linear 
+   * problem at point x.
+   */
+  virtual Vector objectiveDerivative(const Vector& x) const = 0;
+
+  /*
+   * Differentiates the constraints at point x.
+   */
+  virtual Vector constraintsDerivative(const Vector& x) const = 0;
+};
+
+}

gtsam_unstable/linear/SQPSolver.h

@@ -0,0 +1,32 @@
+/**
+ * @file     SQPSolver.h
+ * @brief    This class is used to solve a Non-linear Problem
+ *           (NP) and finds a local solution using SQP. See
+ *           Nocedal Algorithm 18.3 for details.
+ * @author   Ivan Dario Jimenez
+ * @date     2/2/16
+ */
+
+#pragma once
+
+#include <gtsam_unstable/linear/NP.h>
+#include <gtsam_unstable/linear/ActiveSetSolver.h>
+#include <gtsam_unstable/linear/SQPState.h>
+
+class SQPSolver : public ActiveSetSolver {
+  const NP& np_; //!< the nonlinear programming problem
+public:
+  SQPSolver(const NP& np) : np_(np){}
+
+  NPState iterate(const NPState& state) const;
+  /*
+   * This function will optimize the Non-linear problem given a set of initial values
+   */
+  pair<Vector, Vector> optimize(const Vector& initialValues) const;
+
+  /*
+   * This function will attempt to optimize the Non-linear problem without the need
+   * of initial values.
+   */
+  pair<Vector, Vector> optimize();
+};

gtsam_unstable/linear/SQPState.h

@@ -0,0 +1,30 @@
+/**
+ * @file     SQPState.h
+ * @brief    This class is used to store the current state of
+ *           an SQPSolver iteration.
+ * @author   Ivan Dario Jimenez
+ * @date     2/2/16
+ */
+
+#pragma once
+#include <gtsam/base/Matrix.h>
+
+namespace gtsam {
+class SQPState {
+  Vector values;
+  Vector duals;
+  Vector workingSet;
+
+  bool converged;
+  size_t iterations;
+
+  SQPState() :
+    values(), duals(), workingSet(),converged(false), iterations(0) {}
+
+  SQPState(const Vector& initialValues, const Vector& initialDuals,
+           const Vector& initialWorkingSet, const bool _converged,
+           const size_t iterations) :
+    values(initialValues), duals(initialDuals), workingSet(initialWorkingSet),
+    converged(_converged), iterations(iterations) {}
+};
+}

gtsam_unstable/linear/tests/testSQPSolver.cpp

@@ -0,0 +1,11 @@
+#include <gtsam_unstable/linear/NP.h>
+#include <gtsam_unstable/linear/SQPSolver.h>
+#include <gtsam_unstable/linear/SQPState.h>
+
+
+using namespace gtsam;
+
+int main(){
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}