From 8926a1da914cc5c5df4e65cd795e7de165f0303a Mon Sep 17 00:00:00 2001
From: Ivan Jimenez <ijimenez3@gatech.edu>
Date: Tue, 2 Feb 2016 11:03:53 -0500
Subject: [PATCH] [EXPERIMENTAL] Initial SQP Solver. DO NOT BUILD.

---
 .../linear/InfeasibleInitialValues.h          |  2 +-
 gtsam_unstable/linear/NP.h                    | 37 +++++++++++++++++++
 gtsam_unstable/linear/SQPSolver.h             | 32 ++++++++++++++++
 gtsam_unstable/linear/SQPState.h              | 30 +++++++++++++++
 gtsam_unstable/linear/tests/testSQPSolver.cpp | 11 ++++++
 5 files changed, 111 insertions(+), 1 deletion(-)
 create mode 100644 gtsam_unstable/linear/NP.h
 create mode 100644 gtsam_unstable/linear/SQPSolver.h
 create mode 100644 gtsam_unstable/linear/SQPState.h
 create mode 100644 gtsam_unstable/linear/tests/testSQPSolver.cpp

diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h
index 5689c90c5..7c08584c2 100644
--- a/gtsam_unstable/linear/InfeasibleInitialValues.h
+++ b/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> {
diff --git a/gtsam_unstable/linear/NP.h b/gtsam_unstable/linear/NP.h
new file mode 100644
index 000000000..309b4c2f8
--- /dev/null
+++ b/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;
+};
+
+}
diff --git a/gtsam_unstable/linear/SQPSolver.h b/gtsam_unstable/linear/SQPSolver.h
new file mode 100644
index 000000000..f6488ac4a
--- /dev/null
+++ b/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();
+};
diff --git a/gtsam_unstable/linear/SQPState.h b/gtsam_unstable/linear/SQPState.h
new file mode 100644
index 000000000..e272e308a
--- /dev/null
+++ b/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) {}
+};
+}
diff --git a/gtsam_unstable/linear/tests/testSQPSolver.cpp b/gtsam_unstable/linear/tests/testSQPSolver.cpp
new file mode 100644
index 000000000..c259641ed
--- /dev/null
+++ b/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);
+}
\ No newline at end of file