Added an SQPOptimizer class with accompanying test - currently doesn't do much

cpp/Makefile.am

@@ -111,10 +111,11 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL
 # Nonlinear inference 
 headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
+headers += SQPOptimizer.h SQPOptimizer-inl.h  
+headers += NonlinearConstraint.h NonlinearConstraint-inl.h
 sources += NonlinearFactor.cpp 
 sources += NonlinearEquality.h
-sources += NonlinearConstraint.h NonlinearConstraint-inl.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la
 testNonlinearConstraint_SOURCES  = $(example) testNonlinearConstraint.cpp
@@ -123,6 +124,8 @@ testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
 testNonlinearFactorGraph_LDADD   = libgtsam.la
 testNonlinearOptimizer_SOURCES   = $(example) testNonlinearOptimizer.cpp
 testNonlinearOptimizer_LDADD     = libgtsam.la
+testSQPOptimizer_SOURCES   		 = $(example) testSQPOptimizer.cpp
+testSQPOptimizer_LDADD     		 = libgtsam.la
 testNonlinearEquality_SOURCES    = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD      = libgtsam.la
 testSQP_SOURCES                  = $(example) testSQP.cpp

cpp/SQPOptimizer-inl.h

@@ -0,0 +1,33 @@
+/*
+ * @file SQPOptimizer-inl.h
+ * @brief Implementation of the SQP Optimizer
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include "SQPOptimizer.h"
+
+using namespace std;
+namespace gtsam {
+
+/* **************************************************************** */
+template <class G, class C>
+SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
+		shared_config config)
+: graph_(&graph), ordering_(&ordering), config_(config)
+{
+	// TODO: assign a value to the lagrange config
+
+}
+
+/* **************************************************************** */
+template <class G, class C>
+SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
+		shared_config config, shared_vconfig lagrange)
+: graph_(&graph), ordering_(&ordering), config_(config), lagrange_config_(lagrange)
+{
+
+}
+
+}

cpp/SQPOptimizer.h

@@ -0,0 +1,67 @@
+/**
+ * @file SQPOptimizer.h
+ * @brief Interface for a generic SQP-based nonlinear optimization engine
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include "VectorConfig.h"
+
+namespace gtsam {
+
+/**
+ * This class is an engine for performing SQP-based optimization
+ * It stores a graph, a config, and needs a specific ordering, and
+ * then will perform optimization iterations in a functional way.
+ */
+template<class FactorGraph, class Config>
+class SQPOptimizer {
+
+public:
+	// useful for storing configurations
+	typedef boost::shared_ptr<const Config> shared_config;
+	typedef boost::shared_ptr<const VectorConfig> shared_vconfig;
+
+private:
+	// keep const references to the graph and the original ordering
+	const FactorGraph* graph_;
+	const Ordering* ordering_;
+
+	// keep configurations
+	shared_config config_;
+	shared_vconfig lagrange_config_;
+	double error_;
+
+public:
+	/**
+	 * Standard external constructor
+	 * @param graph is the nonlinear graph to optimize
+	 * @param ordering is the elimination ordering to use
+	 * @param config is the initial configuration for the real variables
+	 */
+	SQPOptimizer(const FactorGraph& graph, const Ordering& ordering,
+			shared_config config);
+
+	/**
+	 * Constructor that includes a lagrange initialization.  Primarily
+	 * for internal iterations, but if the user has an idea of what a good
+	 * set of lagrange multipliers is, they can specify them, assuming that
+	 * the naming convention is the same as the internal system.
+	 * @param graph is the nonlinear graph to optimize
+	 * @param ordering is the elimination ordering to use
+	 * @param config is the initial configuration for the real variables
+	 * @param lagrange is the configuration of lagrange multipliers
+	 */
+	SQPOptimizer(const FactorGraph& graph, const Ordering& ordering,
+			shared_config config, shared_vconfig lagrange);
+
+	/// Access functions
+	const FactorGraph* graph() const { return graph_; }
+	const Ordering* ordering() const { return ordering_; }
+	shared_config config() const { return config_; }
+
+};
+
+}
+

cpp/testSQPOptimizer.cpp

@@ -0,0 +1,41 @@
+/*
+ * @file testSQPOptimizer.cpp
+ * @brief tests the optimization algorithm for nonlinear graphs with nonlinear constraints
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include "NonlinearFactorGraph.h"
+#include "NonlinearConstraint.h"
+#include "VectorConfig.h"
+#include "Ordering.h"
+#include "SQPOptimizer.h"
+
+// implementations
+#include "NonlinearConstraint-inl.h"
+#include "SQPOptimizer-inl.h"
+
+using namespace std;
+using namespace gtsam;
+	
+// typedefs
+typedef NonlinearFactorGraph<VectorConfig> NLGraph;
+typedef boost::shared_ptr<VectorConfig> shared_config;
+
+TEST ( SQPOptimizer, basic ) {
+	// create a basic optimizer
+	NLGraph graph;
+	Ordering ordering;
+	shared_config config(new VectorConfig);
+
+	SQPOptimizer<NLGraph, VectorConfig> optimizer(graph, ordering, config);
+
+	// verify components
+	CHECK(assert_equal(graph, *(optimizer.graph())));
+	CHECK(assert_equal(ordering, *(optimizer.ordering())));
+	CHECK(assert_equal(*config, *(optimizer.config())));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */