From e26acc0d8dbc4fad7f7522beab4960ac7000637e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 1 Dec 2009 19:45:47 +0000 Subject: [PATCH] Changed nonlinear constraints to use boost.bind to handle arbitrary function objects for evaluating cost, useful for parameterizing cost functions. --- cpp/NonlinearConstraint-inl.h | 85 +++++++++++++++++++++++---- cpp/NonlinearConstraint.h | 64 +++++++++++++++++--- cpp/testNonlinearConstraint.cpp | 100 +++++++++++++++++++++++++++++--- cpp/testSQPOptimizer.cpp | 94 ++++++++++++++++++++++++++++++ 4 files changed, 318 insertions(+), 25 deletions(-) diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 82dd29be9..f13ff7e0a 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -1,16 +1,41 @@ /* * @file NonlinearConstraint-inl.h * @brief Implementation for NonlinearConstraints - * @author alexgc + * @author Alex Cunningham */ #pragma once #include +#include #include "NonlinearConstraint.h" namespace gtsam { +/* ************************************************************************* */ +// Implementations of base class +/* ************************************************************************* */ + +/* ************************************************************************* */ +template +NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key, + size_t dim_lagrange, + Vector (*g)(const Config& config, const std::list& keys), + bool isEquality) +: NonlinearFactor(zero(dim_lagrange), 1.0), + lagrange_key_(lagrange_key), p_(dim_lagrange), + isEquality_(isEquality), g_(boost::bind(g, _1, _2)) {} + +/* ************************************************************************* */ +template +NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key, + size_t dim_lagrange, + boost::function& keys)> g, + bool isEquality) +: NonlinearFactor(zero(dim_lagrange), 1.0), + lagrange_key_(lagrange_key), p_(dim_lagrange), + g_(g), isEquality_(isEquality) {} + /* ************************************************************************* */ template bool NonlinearConstraint::active(const Config& config) const { @@ -30,13 +55,33 @@ NonlinearConstraint1::NonlinearConstraint1( const std::string& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), - gradG_(gradG), key_(key) { + gradG_(boost::bind(gradG, _1, _2)), key_(key) +{ // set a good lagrange key here // TODO:should do something smart to find a unique one if (lagrange_key == "") this->lagrange_key_ = "L_" + key; this->keys_.push_front(key); - } +} + +/* ************************************************************************* */ +template +NonlinearConstraint1::NonlinearConstraint1( + const std::string& key, + boost::function& keys)> gradG, + boost::function& keys)> g, + size_t dim_constraint, + const std::string& lagrange_key, + bool isEquality) : + NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), + gradG_(gradG), key_(key) +{ + // set a good lagrange key here + // TODO:should do something smart to find a unique one + if (lagrange_key == "") + this->lagrange_key_ = "L_" + key; + this->keys_.push_front(key); +} /* ************************************************************************* */ template @@ -58,8 +103,6 @@ bool NonlinearConstraint1::equals(const Factor& f, double tol) c if (p == NULL) return false; if (key_ != p->key_) return false; if (this->lagrange_key_ != p->lagrange_key_) return false; - if (this->g_ != p->g_) return false; - if (gradG_ != p->gradG_) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } @@ -104,7 +147,32 @@ NonlinearConstraint2::NonlinearConstraint2( const std::string& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), - gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) { + gradG1_(boost::bind(gradG1, _1, _2)), gradG2_(boost::bind(gradG2, _1, _2)), + key1_(key1), key2_(key2) +{ + // set a good lagrange key here + // TODO:should do something smart to find a unique one + if (lagrange_key == "") + this->lagrange_key_ = "L_" + key1 + key2; + this->keys_.push_front(key1); + this->keys_.push_back(key2); +} + +/* ************************************************************************* */ +template +NonlinearConstraint2::NonlinearConstraint2( + const std::string& key1, + boost::function& keys)> gradG1, + const std::string& key2, + boost::function& keys)> gradG2, + boost::function& keys)> g, + size_t dim_constraint, + const std::string& lagrange_key, + bool isEquality) : + NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), + gradG1_(gradG1), gradG2_(gradG2), + key1_(key1), key2_(key2) +{ // set a good lagrange key here // TODO:should do something smart to find a unique one if (lagrange_key == "") @@ -131,9 +199,6 @@ bool NonlinearConstraint2::equals(const Factor& f, double tol) c if (key1_ != p->key1_) return false; if (key2_ != p->key2_) return false; if (this->lagrange_key_ != p->lagrange_key_) return false; - if (this->g_ != p->g_) return false; - if (gradG1_ != p->gradG1_) return false; - if (gradG2_ != p->gradG2_) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } @@ -160,7 +225,7 @@ NonlinearConstraint2::linearize(const Config& config, const VectorConfig this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); // construct the constraint - GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1*g, 0.0)); + GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1.0*g, 0.0)); return std::make_pair(factor, constraint); } diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index d92773cdf..f08cf90f4 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include "NonlinearFactor.h" namespace gtsam { @@ -37,11 +38,12 @@ protected: /** calculates the constraint function of the current config * If the value is zero, the constraint is not active + * Use boost.bind to create the function object * @param config is a configuration of all the variables * @param keys is the set of keys - assumed that the function knows how to use * @return the cost for each of p constraints, arranged in a vector */ - Vector (*g_)(const Config& config, const std::list& keys); + boost::function& keys)> g_; public: @@ -54,10 +56,18 @@ public: NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, Vector (*g)(const Config& config, const std::list& keys), - bool isEquality=true) - : NonlinearFactor(zero(dim_lagrange), 1.0), - lagrange_key_(lagrange_key), p_(dim_lagrange), - isEquality_(isEquality), g_(g) {} + bool isEquality=true); + + /** Constructor - sets a more general cost function using boost::bind directly + * @param lagrange_key is the label for the associated lagrange multipliers + * @param dim_lagrange is the number of associated constraints + * @param g is the cost function for the constraint + * @param isEquality is true if the constraint is an equality constraint + */ + NonlinearConstraint(const std::string& lagrange_key, + size_t dim_lagrange, + boost::function& keys)> g, + bool isEquality=true); /** returns the key used for the Lagrange multipliers */ std::string lagrangeKey() const { return lagrange_key_; } @@ -116,11 +126,12 @@ private: /** * Calculates the gradient of the constraint function * returns a pxn matrix + * Use boost.bind to create the function object * @param config to use for linearization * @param key of selected variable * @return the jacobian of the constraint in terms of key */ - Matrix (*gradG_) (const Config& config, const std::list& keys); + boost::function& keys)> gradG_; /** key for the constrained variable */ std::string key_; @@ -144,6 +155,23 @@ public: const std::string& lagrange_key="", bool isEquality=true); + /** + * Basic constructor with boost function pointers + * @param key is the identifier for the variable constrained + * @param gradG gives the gradient of the constraint function + * @param g is the constraint function as a boost function pointer + * @param dim_constraint is the size of the constraint (p) + * @param lagrange_key is the identifier for the lagrange multiplier + * @param isEquality is true if the constraint is an equality constraint + */ + NonlinearConstraint1( + const std::string& key, + boost::function& keys)> gradG, + boost::function& keys)> g, + size_t dim_constraint, + const std::string& lagrange_key="", + bool isEquality=true); + /** Print */ void print(const std::string& s = "") const; @@ -177,8 +205,8 @@ private: * @param key of selected variable * @return the jacobian of the constraint in terms of key */ - Matrix (*gradG1_) (const Config& config, const std::list& keys); - Matrix (*gradG2_) (const Config& config, const std::list& keys); + boost::function& keys)> gradG1_; + boost::function& keys)> gradG2_; /** keys for the constrained variables */ std::string key1_; @@ -205,6 +233,26 @@ public: const std::string& lagrange_key="", bool isEquality=true); + /** + * Basic constructor with direct function objects + * Use boost.bind to construct the function objects + * @param key is the identifier for the variable constrained + * @param gradG gives the gradient of the constraint function + * @param g is the constraint function + * @param dim_constraint is the size of the constraint (p) + * @param lagrange_key is the identifier for the lagrange multiplier + * @param isEquality is true if the constraint is an equality constraint + */ + NonlinearConstraint2( + const std::string& key1, + boost::function& keys)> gradG1, + const std::string& key2, + boost::function& keys)> gradG2, + boost::function& keys)> g, + size_t dim_constraint, + const std::string& lagrange_key="", + bool isEquality=true); + /** Print */ void print(const std::string& s = "") const; diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 150f2f49f..f90970916 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -4,6 +4,7 @@ * @author Alex Cunningham */ +#include #include #include #include @@ -192,10 +193,7 @@ Matrix grad_g(const VectorConfig& config, const list& keys) { Vector g_func(const VectorConfig& config, const list& keys) { double x = config[keys.front()](0); double g = x*x-5; - if (g > 0) - return Vector_(1, 0.0); // return exactly zero - else - return Vector_(1, g); // return the actual cost + return Vector_(1, g); // return the actual cost } } // \namespace test1 @@ -208,14 +206,14 @@ TEST( NonlinearConstraint1, unary_inequality ) { // get configurations to use for evaluation VectorConfig config1, config2; - config1.insert("x", Vector_(1, 10.0)); // should have zero error + config1.insert("x", Vector_(1, 10.0)); // should be inactive config2.insert("x", Vector_(1, 1.0)); // should have nonzero error // check error - Vector actError1 = c1.error_vector(config1); + CHECK(!c1.active(config1)); Vector actError2 = c1.error_vector(config2); - CHECK(actError1(0) == 0.0); // NOTE: using exact comparison here, as this value is forced CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9))); + CHECK(c1.active(config2)); } /* ************************************************************************* */ @@ -252,6 +250,94 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { CHECK(assert_equal(*actFactor2, expFactor)); CHECK(assert_equal(*actConstraint2, expConstraint)); } + +/* ************************************************************************* */ +// Binding arbitrary functions +/* ************************************************************************* */ +namespace unary_binding_functions { +/** p = 1, gradG(x) = 2*x */ +Matrix grad_g(double coeff, const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); + return Matrix_(1,1, coeff*x); +} + +/** p = 1, g(x) x^2 - r > 0 */ +Vector g_func(double r, const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); + double g = x*x-r; + return Vector_(1, g); // return the actual cost +} +} // \namespace unary_binding_functions + +/* ************************************************************************* */ +TEST( NonlinearConstraint1, unary_binding ) { + size_t p = 1; + double coeff = 2; + double radius = 5; + NonlinearConstraint1 c1("x", + boost::bind(unary_binding_functions::grad_g, coeff, _1, _2), + boost::bind(unary_binding_functions::g_func, radius, _1, _2), + p, "L_x1", + false); // inequality constraint + + // get configurations to use for evaluation + VectorConfig config1, config2; + config1.insert("x", Vector_(1, 10.0)); // should have zero error + config2.insert("x", Vector_(1, 1.0)); // should have nonzero error + + // check error + CHECK(!c1.active(config1)); + Vector actError2 = c1.error_vector(config2); + CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9))); + CHECK(c1.active(config2)); +} + +namespace binary_binding_functions { +/** gradient for x, gradG(x,y) in x: 2x*/ +Matrix grad_g1(double c, const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); + return Matrix_(1,1, c*x); +} + +/** gradient for y, gradG(x,y) in y: -1 */ +Matrix grad_g2(double c, const VectorConfig& config, const list& keys) { + double x = config[keys.back()](0); + return Matrix_(1,1, -1.0*c); +} + +/** p = 1, g(x) = x^2-5 -y = 0 */ +Vector g_func(double r, const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); + double y = config[keys.back()](0); + return Vector_(1, x*x-r-y); +} +} // \namespace test2 + +/* ************************************************************************* */ +TEST( NonlinearConstraint2, binary_binding ) { + // construct a constraint on x and y + // the lagrange multipliers will be expected on L_xy + // and there is only one multiplier + size_t p = 1; + double a = 2.0; + double b = 1.0; + double r = 5.0; + NonlinearConstraint2 c1( + "x", boost::bind(binary_binding_functions::grad_g1, a, _1, _2), + "y", boost::bind(binary_binding_functions::grad_g2, b, _1, _2), + boost::bind(binary_binding_functions::g_func, r, _1, _2), p, "L_xy"); + + // get a configuration to use for finding the error + VectorConfig config; + config.insert("x", Vector_(1, 1.0)); + config.insert("y", Vector_(1, 2.0)); + + // calculate the error + Vector actual = c1.error_vector(config); + Vector expected = Vector_(1.0, -6.0); + CHECK(assert_equal(actual, expected, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index e6cedf6c0..a744d6a94 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -7,6 +7,7 @@ #include #include // for operator += #include // for insert +#include #include #include #include @@ -348,7 +349,100 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) { CHECK(assert_equal(exp2, *(final.config()))); } +/* ********************************************************************* */ +// Use boost bind to parameterize the function +namespace sqp_avoid2 { +// binary avoidance constraint +/** g(x) = ||x2-obs||^2 - radius^2 > 0 */ +Vector g_func(double radius, const VectorConfig& config, const list& keys) { + Vector delta = config[keys.front()]-config[keys.back()]; + double dist2 = sum(emul(delta, delta)); + double thresh = radius*radius; + return Vector_(1, dist2-thresh); +} +/** gradient at pose */ +Matrix grad_g1(const VectorConfig& config, const list& keys) { + Vector x2 = config[keys.front()], obs = config[keys.back()]; + Vector grad = 2.0*(x2-obs); + return Matrix_(1,2, grad(0), grad(1)); +} + +/** gradient at obstacle */ +Matrix grad_g2(const VectorConfig& config, const list& keys) { + Vector x2 = config[keys.front()], obs = config[keys.back()]; + Vector grad = -2.0*(x2-obs); + return Matrix_(1,2, grad(0), grad(1)); +} +} + +pair obstacleAvoidGraphGeneral() { + // fix start, end, obstacle positions + VectorConfig feasible; + feasible.insert("x1", Vector_(2, 0.0, 0.0)); + feasible.insert("x3", Vector_(2, 10.0, 0.0)); + feasible.insert("obs", Vector_(2, 5.0, -0.5)); + shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare)); + shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare)); + shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare)); + + // measurement from x1 to x2 + Vector x1x2 = Vector_(2, 5.0, 0.0); + double sigma1 = 0.1; + shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); + + // measurement from x2 to x3 + Vector x2x3 = Vector_(2, 5.0, 0.0); + double sigma2 = 0.1; + shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); + + double radius = 1.0; + + // create a binary inequality constraint that forces the middle point away from + // the obstacle + shared_NLC2 c1(new NLC2("x2", boost::bind(sqp_avoid2::grad_g1, _1, _2), + "obs", boost::bind(sqp_avoid2::grad_g2, _1, _2), + boost::bind(sqp_avoid2::g_func, radius, _1, _2), 1, "L_x2obs", false)); + + // construct the graph + NLGraph graph; + graph.push_back(e1); + graph.push_back(e2); + graph.push_back(e3); + graph.push_back(c1); + graph.push_back(f1); + graph.push_back(f2); + + return make_pair(graph, feasible); +} + +/* ********************************************************************* */ +TEST ( SQPOptimizer, inequality_avoid_iterative_bind ) { + // create the graph + NLGraph graph; VectorConfig feasible; + boost::tie(graph, feasible) = obstacleAvoidGraphGeneral(); + + // 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); }