From 0ff7e3a5d954a1e06cc25e61fd18e9c47f8ca6e9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 28 Nov 2009 19:18:02 +0000 Subject: [PATCH] Changed interface on constraint g(x) and grad_g(x) functions to take a list of keys, so that all of the variables in a factor can be used as necessary. Moved g(x) into base NonlinearConstraint class and some cleanup. --- cpp/NonlinearConstraint-inl.h | 35 ++++++++++--------- cpp/NonlinearConstraint.h | 61 +++++++++++++-------------------- cpp/testNonlinearConstraint.cpp | 31 +++++++++-------- cpp/testSQP.cpp | 22 ++++++------ cpp/testSQPOptimizer.cpp | 22 ++++++------ 5 files changed, 81 insertions(+), 90 deletions(-) diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index f7ef45f96..6327dad2b 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -27,17 +27,18 @@ bool NonlinearConstraint::active(const Config& config) const { template NonlinearConstraint1::NonlinearConstraint1( const std::string& key, - Matrix (*gradG)(const Config& config, const std::string& key), - Vector (*g)(const Config& config, const std::string& key), + Matrix (*gradG)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : - NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - g_(g), gradG_(gradG), key_(key) { + 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); } /* ************************************************************************* */ @@ -60,7 +61,7 @@ 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 (g_ != p->g_) 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_; @@ -74,10 +75,10 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig Vector lambda = lagrange[this->lagrange_key_]; // find the error - Vector g = g_(config, key_); + Vector g = g_(config, this->keys()); // construct the gradient - Matrix grad = gradG_(config, key_); + Matrix grad = gradG_(config, this->keys()); // construct probabilistic factor Matrix A1 = vector_scale(grad, lambda); @@ -98,19 +99,21 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig template NonlinearConstraint2::NonlinearConstraint2( const std::string& key1, - Matrix (*gradG1)(const Config& config, const std::string& key), + Matrix (*gradG1)(const Config& config, const std::list& keys), const std::string& key2, - Matrix (*gradG2)(const Config& config, const std::string& key), - Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), + Matrix (*gradG2)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : - NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) { + 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 == "") this->lagrange_key_ = "L_" + key1 + key2; + this->keys_.push_front(key1); + this->keys_.push_back(key2); } /* ************************************************************************* */ @@ -131,7 +134,7 @@ 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 (g_ != p->g_) 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; @@ -146,11 +149,11 @@ NonlinearConstraint2::linearize(const Config& config, const VectorConfig Vector lambda = lagrange[this->lagrange_key_]; // find the error - Vector g = g_(config, key1_, key2_); + Vector g = g_(config, this->keys()); // construct the gradients - Matrix grad1 = gradG1_(config, key1_); - Matrix grad2 = gradG2_(config, key2_); + Matrix grad1 = gradG1_(config, this->keys()); + Matrix grad2 = gradG2_(config, this->keys()); // construct probabilistic factor Matrix A1 = vector_scale(grad1, lambda); diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 98910bbd3..d92773cdf 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -35,17 +35,29 @@ protected: /** type of constraint */ bool isEquality_; + /** calculates the constraint function of the current config + * If the value is zero, the constraint is not active + * @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); + public: /** Constructor - sets the cost function and the lagrange multipliers * @param lagrange_key is the label for the associated lagrange multipliers * @param dim_lagrange is the number of associated constraints * @param isEquality is true if the constraint is an equality constraint + * @param g is the cost function for the constraint */ - NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, - bool isEquality=true) + 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) {} + lagrange_key_(lagrange_key), p_(dim_lagrange), + isEquality_(isEquality), g_(g) {} /** returns the key used for the Lagrange multipliers */ std::string lagrangeKey() const { return lagrange_key_; } @@ -63,7 +75,7 @@ public: virtual bool equals(const Factor& f, double tol=1e-9) const=0; /** error function - returns the result of the constraint function */ - virtual inline Vector error_vector(const Config& c) const { return zero(1); } + inline Vector error_vector(const Config& c) const { return g_(c, this->keys()); } /** * Determines whether the constraint is active given a particular configuration @@ -100,13 +112,6 @@ template class NonlinearConstraint1 : public NonlinearConstraint { private: - /** calculates the constraint function of the current config - * If the value is zero, the constraint is not active - * @param config is a configuration of all the variables - * @param key is the id for the selected variable - * @return the cost for each of p constraints, arranged in a vector - */ - Vector (*g_)(const Config& config, const std::string& key); /** * Calculates the gradient of the constraint function @@ -115,7 +120,7 @@ private: * @param key of selected variable * @return the jacobian of the constraint in terms of key */ - Matrix (*gradG_) (const Config& config, const std::string& key); + Matrix (*gradG_) (const Config& config, const std::list& keys); /** key for the constrained variable */ std::string key_; @@ -133,8 +138,8 @@ public: */ NonlinearConstraint1( const std::string& key, - Matrix (*gradG)(const Config& config, const std::string& key), - Vector (*g)(const Config& config, const std::string& key), + Matrix (*gradG)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -145,11 +150,6 @@ public: /** Check if two factors are equal */ bool equals(const Factor& f, double tol=1e-9) const; - /** error function - returns the result of the constraint function */ - inline Vector error_vector(const Config& c) const { - return g_(c, key_); - } - /** * Linearize using a real Config and a VectorConfig of Lagrange multipliers * Returns the two separate Gaussian factors to solve @@ -168,14 +168,6 @@ template class NonlinearConstraint2 : public NonlinearConstraint { private: - /** calculates the constraint function of the current config - * If the value is zero, the constraint is not active - * @param config is a configuration of all the variables - * @param key1 is the id for the first variable - * @param key2 is the id for the second variable - * @return the cost for each of p constraints, arranged in a vector - */ - Vector (*g_)(const Config& config, const std::string& key1, const std::string& key2); /** * Calculates the gradients of the constraint function in terms of @@ -185,8 +177,8 @@ private: * @param key of selected variable * @return the jacobian of the constraint in terms of key */ - Matrix (*gradG1_) (const Config& config, const std::string& key); - Matrix (*gradG2_) (const Config& config, const std::string& key); + Matrix (*gradG1_) (const Config& config, const std::list& keys); + Matrix (*gradG2_) (const Config& config, const std::list& keys); /** keys for the constrained variables */ std::string key1_; @@ -205,10 +197,10 @@ public: */ NonlinearConstraint2( const std::string& key1, - Matrix (*gradG1)(const Config& config, const std::string& key), + Matrix (*gradG1)(const Config& config, const std::list& keys), const std::string& key2, - Matrix (*gradG2)(const Config& config, const std::string& key), - Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), + Matrix (*gradG2)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -219,11 +211,6 @@ public: /** Check if two factors are equal */ bool equals(const Factor& f, double tol=1e-9) const; - /** error function - returns the result of the constraint function */ - inline Vector error_vector(const Config& c) const { - return g_(c, key1_, key2_); - } - /** * Linearize using a real Config and a VectorConfig of Lagrange multipliers * Returns the two separate Gaussian factors to solve diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 28ece8eb5..fc1624bc0 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -9,6 +9,7 @@ #include #include +using namespace std; using namespace gtsam; /* ************************************************************************* */ @@ -16,14 +17,14 @@ using namespace gtsam; /* ************************************************************************* */ namespace test1 { /** p = 1, gradG(x) = 2x */ -Matrix grad_g(const VectorConfig& config, const std::string& key) { - double x = config[key](0); +Matrix grad_g(const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); return Matrix_(1,1, 2*x); } /** p = 1, g(x) = x^2-5 = 0 */ -Vector g_func(const VectorConfig& config, const std::string& key) { - double x = config[key](0); +Vector g_func(const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); return Vector_(1, x*x-5); } } // \namespace test1 @@ -89,21 +90,21 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) { /* ************************************************************************* */ namespace test2 { /** gradient for x, gradG(x,y) in x: 2x*/ -Matrix grad_g1(const VectorConfig& config, const std::string& key) { - double x = config[key](0); +Matrix grad_g1(const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); return Matrix_(1,1, 2.0*x); } /** gradient for y, gradG(x,y) in y: -1 */ -Matrix grad_g2(const VectorConfig& config, const std::string& key) { - double x = config[key](0); +Matrix grad_g2(const VectorConfig& config, const list& keys) { + double x = config[keys.back()](0); return Matrix_(1,1, -1.0); } /** p = 1, g(x) = x^2-5 -y = 0 */ -Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { - double x = config[key1](0); - double y = config[key2](0); +Vector g_func(const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); + double y = config[keys.back()](0); return Vector_(1, x*x-5.0-y); } } // \namespace test2 @@ -182,14 +183,14 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) { /* ************************************************************************* */ namespace inequality_unary { /** p = 1, gradG(x) = 2*x */ -Matrix grad_g(const VectorConfig& config, const std::string& key) { - double x = config[key](0); +Matrix grad_g(const VectorConfig& config, const list& keys) { + double x = config[keys.front()](0); return Matrix_(1,1, 2*x); } /** p = 1, g(x) x^2 - 5 > 0 */ -Vector g_func(const VectorConfig& config, const std::string& key) { - double x = config[key](0); +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 diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 97b2299f6..a2034b64d 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -320,17 +320,17 @@ TEST (SQP, two_pose_truth ) { namespace sqp_test1 { // binary constraint between landmarks /** g(x) = x-y = 0 */ -Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { - return config[key1]-config[key2]; +Vector g_func(const VectorConfig& config, const list& keys) { + return config[keys.front()]-config[keys.back()]; } /** gradient at l1 */ -Matrix grad_g1(const VectorConfig& config, const std::string& key) { +Matrix grad_g1(const VectorConfig& config, const list& keys) { return eye(2); } /** gradient at l2 */ -Matrix grad_g2(const VectorConfig& config, const std::string& key) { +Matrix grad_g2(const VectorConfig& config, const list& keys) { return -1*eye(2); } } // \namespace sqp_test1 @@ -338,12 +338,12 @@ Matrix grad_g2(const VectorConfig& config, const std::string& key) { namespace sqp_test2 { // Unary Constraint on x1 /** g(x) = x -[1;1] = 0 */ -Vector g_func(const VectorConfig& config, const std::string& key) { - return config[key]-Vector_(2, 1.0, 1.0); +Vector g_func(const VectorConfig& config, const list& keys) { + return config[keys.front()]-Vector_(2, 1.0, 1.0); } /** gradient at x1 */ -Matrix grad_g(const VectorConfig& config, const std::string& key) { +Matrix grad_g(const VectorConfig& config, const list& keys) { return eye(2); } } // \namespace sqp_test2 @@ -635,17 +635,17 @@ int getNum(const string& key) { namespace sqp_stereo { // binary constraint between landmarks /** g(x) = x-y = 0 */ -Vector g_func(const VSLAMConfig& config, const std::string& key1, const std::string& key2) { - return config.landmarkPoint(getNum(key1)).vector()-config.landmarkPoint(getNum(key2)).vector(); +Vector g_func(const VSLAMConfig& config, const list& keys) { + return config.landmarkPoint(getNum(keys.front())).vector()-config.landmarkPoint(getNum(keys.back())).vector(); } /** gradient at l1 */ -Matrix grad_g1(const VSLAMConfig& config, const std::string& key) { +Matrix grad_g1(const VSLAMConfig& config, const list& keys) { return eye(3); } /** gradient at l2 */ -Matrix grad_g2(const VSLAMConfig& config, const std::string& key) { +Matrix grad_g2(const VSLAMConfig& config, const list& keys) { return -1.0*eye(3); } } // \namespace sqp_test1 diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 1f1ba78fe..de11fc8ef 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -53,17 +53,17 @@ TEST ( SQPOptimizer, basic ) { namespace sqp_LinearMapWarp2 { // binary constraint between landmarks /** g(x) = x-y = 0 */ -Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { - return config[key1]-config[key2]; +Vector g_func(const VectorConfig& config, const list& keys) { + return config[keys.front()]-config[keys.back()]; } /** gradient at l1 */ -Matrix grad_g1(const VectorConfig& config, const std::string& key) { +Matrix grad_g1(const VectorConfig& config, const list& keys) { return eye(2); } /** gradient at l2 */ -Matrix grad_g2(const VectorConfig& config, const std::string& key) { +Matrix grad_g2(const VectorConfig& config, const list& keys) { return -1*eye(2); } } // \namespace sqp_LinearMapWarp2 @@ -71,12 +71,12 @@ Matrix grad_g2(const VectorConfig& config, const std::string& key) { namespace sqp_LinearMapWarp1 { // Unary Constraint on x1 /** g(x) = x -[1;1] = 0 */ -Vector g_func(const VectorConfig& config, const std::string& key) { - return config[key]-Vector_(2, 1.0, 1.0); +Vector g_func(const VectorConfig& config, const list& keys) { + return config[keys.front()]-Vector_(2, 1.0, 1.0); } /** gradient at x1 */ -Matrix grad_g(const VectorConfig& config, const std::string& key) { +Matrix grad_g(const VectorConfig& config, const list& keys) { return eye(2); } } // \namespace sqp_LinearMapWarp12 @@ -223,8 +223,8 @@ double radius = 1.0; // binary avoidance constraint /** g(x) = ||x2-obs||^2 > radius^2 */ -Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { - Vector delta = config[key1]-config[key2]; +Vector g_func(const VectorConfig& config, const list& keys) { + Vector delta = config[keys.front()]-config[keys.back()]; double dist2 = sum(emul(delta, delta)); double thresh = radius*radius; if (dist2 <= thresh) @@ -234,13 +234,13 @@ Vector g_func(const VectorConfig& config, const std::string& key1, const std::st } /** gradient at pose */ -Matrix grad_g1(const VectorConfig& config, const std::string& key) { +Matrix grad_g1(const VectorConfig& config, const list& keys) { Matrix grad; return grad; } /** gradient at obstacle */ -Matrix grad_g2(const VectorConfig& config, const std::string& key) { +Matrix grad_g2(const VectorConfig& config, const list& keys) { return -1*eye(1); } }