From 0b333630e70b2ff5e5d7be0903b630e608758b96 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 23 Nov 2009 19:57:35 +0000 Subject: [PATCH] Added an SQPOptimizer class with accompanying test - currently doesn't do much --- cpp/Makefile.am | 7 +++-- cpp/SQPOptimizer-inl.h | 33 ++++++++++++++++++++ cpp/SQPOptimizer.h | 67 ++++++++++++++++++++++++++++++++++++++++ cpp/testSQPOptimizer.cpp | 41 ++++++++++++++++++++++++ 4 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 cpp/SQPOptimizer-inl.h create mode 100644 cpp/SQPOptimizer.h create mode 100644 cpp/testSQPOptimizer.cpp diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 3398754a6..618eae62c 100644 --- a/cpp/Makefile.am +++ b/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 diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h new file mode 100644 index 000000000..b245442c8 --- /dev/null +++ b/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 +SQPOptimizer::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 +SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, + shared_config config, shared_vconfig lagrange) +: graph_(&graph), ordering_(&ordering), config_(config), lagrange_config_(lagrange) +{ + +} + +} diff --git a/cpp/SQPOptimizer.h b/cpp/SQPOptimizer.h new file mode 100644 index 000000000..31f929831 --- /dev/null +++ b/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 SQPOptimizer { + +public: + // useful for storing configurations + typedef boost::shared_ptr shared_config; + typedef boost::shared_ptr 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_; } + +}; + +} + diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp new file mode 100644 index 000000000..dfecae066 --- /dev/null +++ b/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 +#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 NLGraph; +typedef boost::shared_ptr shared_config; + +TEST ( SQPOptimizer, basic ) { + // create a basic optimizer + NLGraph graph; + Ordering ordering; + shared_config config(new VectorConfig); + + SQPOptimizer 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); } +/* ************************************************************************* */