From 1799f5938844e60fad2cc9f21227b4d6fe560dc2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 28 Nov 2009 22:49:14 +0000 Subject: [PATCH] Added a function for the SQPOptimizer that will iterate until convergence. At the moment, the convergence conditions are quite simple (error below threshold or too many iterations). The system does, however, strictly limit the number of iterations. --- cpp/SQPOptimizer-inl.h | 59 ++++++++++++++++++++++++++++++++++++++-- cpp/SQPOptimizer.h | 24 ++++++++++++++++ cpp/testSQPOptimizer.cpp | 29 ++++++++++++++++++++ 3 files changed, 110 insertions(+), 2 deletions(-) diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index f6d0c98a9..6111ca310 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -22,12 +22,35 @@ using namespace boost::assign; namespace gtsam { +/* **************************************************************** */ +template +double constraintError(const G& graph, const C& config) { + // local typedefs + typedef typename G::const_iterator const_iterator; + typedef NonlinearConstraint NLConstraint; + typedef boost::shared_ptr shared_c; + + // accumulate error + double error = 0; + + // find the constraints + for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) { + const shared_c constraint = boost::shared_dynamic_cast(*factor); + if (constraint != NULL) { + Vector e = constraint->error_vector(config); + error += inner_prod(trans(e),e); + } + } + return error; +} + /* **************************************************************** */ template SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, shared_config config) : graph_(&graph), ordering_(&ordering), full_ordering_(ordering), - config_(config), lagrange_config_(new VectorConfig), error_(graph.error(*config)) + config_(config), lagrange_config_(new VectorConfig), error_(graph.error(*config)), + constraint_error_(constraintError(graph, *config)) { // local typedefs typedef typename G::const_iterator const_iterator; @@ -53,7 +76,8 @@ template SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, shared_config config, shared_vconfig lagrange) : graph_(&graph), ordering_(&ordering), full_ordering_(ordering), - config_(config), lagrange_config_(lagrange), error_(graph.error(*config)) + config_(config), lagrange_config_(lagrange), error_(graph.error(*config)), + constraint_error_(constraintError(graph, *config)) { } @@ -109,6 +133,37 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { return SQPOptimizer(*graph_, full_ordering_, newConfig, newLamConfig); } +/* **************************************************************** */ +template +SQPOptimizer SQPOptimizer::iterateSolve(double relThresh, double absThresh, + double constraintThresh, size_t maxIterations, Verbosity v) const { + bool verbose = v == SQPOptimizer::FULL; + + // do an iteration + SQPOptimizer next = iterate(v); + + // if converged or out of iterations, return result + if (maxIterations == 1 || + next.checkConvergence(relThresh, absThresh, constraintThresh, + error_, constraint_error_)) + return next; + else // otherwise, recurse with a lower maxIterations + return next.iterateSolve(relThresh, absThresh, constraintThresh, + maxIterations-1, v); +} + +/* **************************************************************** */ +template +bool SQPOptimizer::checkConvergence(double relThresh, double absThresh, + double constraintThresh, double full_error, double constraint_error) const { + // if error sufficiently low, then the system has converged + if (error_ < absThresh && constraint_error_ < constraintThresh) + return true; + + // TODO: determine other cases + return false; +} + /* **************************************************************** */ template void SQPOptimizer::print(const std::string& s) { diff --git a/cpp/SQPOptimizer.h b/cpp/SQPOptimizer.h index d94adfa7f..6f1f65b8e 100644 --- a/cpp/SQPOptimizer.h +++ b/cpp/SQPOptimizer.h @@ -83,6 +83,30 @@ public: */ SQPOptimizer iterate(Verbosity verbosity=SILENT) const; + /** + * Iterates recursively until converence occurs + * @param relThresh minimum change in error between iterations + * @param absThresh minimum error necessary to converge + * @param constraintThresh minimum constraint error to be feasible + * @param maxIterations is the maximum number of iterations + * @param verbosity controls output print statements + * @return a new optimization object with final values + */ + SQPOptimizer + iterateSolve(double relThresh, double absThresh, double constraintThresh, + size_t maxIterations = 10, Verbosity verbosity=SILENT) const; + + /** + * Checks whether convergence has occurred, and returns true if + * the solution will not get better, based on the previous error conditions. + * @param full_error is the error all the factors and constraints + * @param constraint_error is the error of just the constraints + * @param relThresh is the relative threshold between + * @return true if the problem has converged + */ + bool checkConvergence(double relThresh, double absThresh, + double constraintThresh, double full_error, double constraint_error) const; + /** Standard print function with optional name */ void print(const std::string& s); }; diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index a69d46991..102348210 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -323,6 +323,35 @@ TEST ( SQPOptimizer, inequality_avoid ) { CHECK(assert_equal(exp2, *(after2ndIteration.config()))); } +/* ********************************************************************* */ +TEST ( SQPOptimizer, inequality_avoid_iterative ) { + // create the graph + NLGraph graph; VectorConfig feasible; + boost::tie(graph, feasible) = obstacleAvoidGraph(); + + // create the rest of the config + shared_config init(new VectorConfig(feasible)); + init->insert("x2", Vector_(2, 5.0, 100.0)); + + // create an ordering + Ordering ord; + ord += "x1", "x2", "x3", "obs"; + + // create an optimizer + Optimizer optimizer(graph, ord, init); + + double relThresh = 1e-5; // minimum change in error between iterations + double absThresh = 1e-5; // minimum error necessary to converge + double constraintThresh = 1e-9; // minimum constraint error to be feasible + Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh); + + // verify + VectorConfig exp2(feasible); + exp2.insert("x2", Vector_(2, 5.0, 0.5)); + CHECK(assert_equal(exp2, *(final.config()))); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }