From 714edb57f014ffe626df5ce4141d55fe492b3cc2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 20 Nov 2009 03:50:48 +0000 Subject: [PATCH] Basic implementation of a binary nonlinear constraint, with working linearization of a binary constraint. --- cpp/NonlinearConstraint-inl.h | 70 +++++++++++++++-- cpp/NonlinearConstraint.h | 134 ++++++++++++++++++-------------- cpp/testNonlinearConstraint.cpp | 102 ++++++++++++++++++++++-- 3 files changed, 236 insertions(+), 70 deletions(-) diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 46f509f8a..d77407eb5 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -11,18 +11,20 @@ namespace gtsam { -/** - * Implementations of unary nonlinear constraints - */ +/* ************************************************************************* */ +// Implementations of unary nonlinear constraints +/* ************************************************************************* */ +/* ************************************************************************* */ template void NonlinearConstraint1::print(const std::string& s) const { - std::cout << "NonlinearConstraint [" << s << "]:\n" - << " Variable: " << key_ << "\n" + std::cout << "NonlinearConstraint1 [" << s << "]:\n" + << " key: " << key_ << "\n" << " p: " << this->p_ << "\n" << " lambda key: " << this->lagrange_key_ << std::endl; } +/* ************************************************************************* */ template bool NonlinearConstraint1::equals(const Factor& f, double tol) const { const NonlinearConstraint1* p = dynamic_cast*> (&f); @@ -34,6 +36,7 @@ bool NonlinearConstraint1::equals(const Factor& f, double tol) c return this->p_ == p->p_; } +/* ************************************************************************* */ template std::pair NonlinearConstraint1::linearize(const Config& config, const VectorConfig& lagrange) const { @@ -52,7 +55,62 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); // construct the constraint - GaussianFactor::shared_ptr constraint(new GaussianFactor("x", grad, g, 0.0)); + GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, g, 0.0)); + + return std::make_pair(factor, constraint); +} + +/* ************************************************************************* */ +// Implementations of binary nonlinear constraints +/* ************************************************************************* */ + +/* ************************************************************************* */ +template +void NonlinearConstraint2::print(const std::string& s) const { + std::cout << "NonlinearConstraint2 [" << s << "]:\n" + << " key1: " << key1_ << "\n" + << " key2: " << key2_ << "\n" + << " p: " << this->p_ << "\n" + << " lambda key: " << this->lagrange_key_ << std::endl; +} + +/* ************************************************************************* */ +template +bool NonlinearConstraint2::equals(const Factor& f, double tol) const { + const NonlinearConstraint2* p = dynamic_cast*> (&f); + if (p == NULL) return false; + 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 (gradG1_ != p->gradG1_) return false; + if (gradG2_ != p->gradG2_) return false; + return this->p_ == p->p_; +} + +/* ************************************************************************* */ +template +std::pair +NonlinearConstraint2::linearize(const Config& config, const VectorConfig& lagrange) const { + // extract lagrange multiplier + Vector lambda = lagrange[this->lagrange_key_]; + + // find the error + Vector g = g_(config, key1_, key2_); + + // construct the gradients + Matrix grad1 = gradG1_(config, key1_); + Matrix grad2 = gradG2_(config, key2_); + + // construct probabilistic factor + Matrix A1 = vector_scale(grad1, lambda); + Matrix A2 = vector_scale(grad2, lambda); + GaussianFactor::shared_ptr factor(new + GaussianFactor(key1_, A1, key2_, A2, + this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); + + // construct the constraint + GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, g, 0.0)); return std::make_pair(factor, constraint); } diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index dcc7ac8bc..591b7b80a 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -151,65 +151,83 @@ public: linearize(const Config& config, const VectorConfig& lagrange) const; }; +/** + * A binary constraint with arbitrary cost and gradient functions + */ +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); -//template -//class NonlinearConstraint2 : public NonlinearConstraint { -// -//private: -// /** -// * Calculates the gradient of the constraint function -// * returns a pxn matrix for x1 -// * @param config -// */ -// Matrix (*gradG1_) (const Config& config); -// -// /** -// * Calculates the gradient of the constraint function -// * returns a pxn matrix for x2 -// * @param config -// */ -// Matrix (*gradG2_) (const Config& config); -// -// /** keys for the constrained variables */ -// std::string key1_; -// std::string key2_; -// -//public: -// -// /** -// * Basic constructor -// * @param key1 is the first variable -// * @param gradG1 gives the gradient for the first variable -// * @param key2 is the first variable -// * @param gradG2 gives the gradient for the first variable -// * @param g is the constraint function -// * @param lambdas is vector of size p with Lagrange multipliers -// */ -// NonlinearConstraint2( -// const std::string& key1, -// Matrix (*gradG1)(const Config& config), -// const std::string& key2, -// Matrix (*gradG2)(const Config& config), -// Vector (*g)(const Config& config), -// const Vector& lambdas) : -// NonlinearConstraint(lambdas, g), -// gradG1_(gradG1), key1_(key1), -// gradG2_(gradG2), key2_(key2) {} -// -// /** Print */ -// void print(const std::string& s = "") const {} -// -// /** Check if two factors are equal */ -// bool equals(const Factor& f, double tol=1e-9) const { -// return true; -// } -// -// /** Linearize a non-linearFactor2 to get a linearFactor2 */ -// boost::shared_ptr linearize(const Config& c) const { -// boost::shared_ptr ret; -// return ret; -// } -//}; + /** + * Calculates the gradients of the constraint function in terms of + * the first and second variables + * returns a pxn matrix + * @param config to use for linearization + * @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); + + /** keys for the constrained variables */ + std::string key1_; + std::string key2_; + +public: + + /** + * Basic constructor + * @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 + */ + NonlinearConstraint2( + const std::string& key1, + Matrix (*gradG1)(const Config& config, const std::string& key), + 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), + size_t dim_constraint, + const std::string& lagrange_key="") : + NonlinearConstraint(lagrange_key, dim_constraint), + g_(g), 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; + } + + /** Print */ + void print(const std::string& s = "") const; + + /** 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 + * @param config is the real Config of the real variables + * @param lagrange is the VectorConfig of lagrange multipliers + * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint) + */ + std::pair + linearize(const Config& config, const VectorConfig& lagrange) const; +}; } diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 32ac50a83..87f75c206 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -29,7 +29,7 @@ Vector g_func(const VectorConfig& config, const std::string& key) { } // \namespace test1 /* ************************************************************************* */ -TEST( NonlinearConstraint, unary_scalar_construction ) { +TEST( NonlinearConstraint1, unary_scalar_construction ) { // construct a constraint on x // the lagrange multipliers will be expected on L_x1 // and there is only one multiplier @@ -47,10 +47,7 @@ TEST( NonlinearConstraint, unary_scalar_construction ) { } /* ************************************************************************* */ -TEST( NonlinearConstraint, unary_scalar_linearize ) { - // construct a constraint on x - // the lagrange multipliers will be expected on L_x1 - // and there is only one multiplier +TEST( NonlinearConstraint1, unary_scalar_linearize ) { size_t p = 1; NonlinearConstraint1 c1("x", *test1::grad_g, *test1::g_func, p, "L_x1"); @@ -74,7 +71,7 @@ TEST( NonlinearConstraint, unary_scalar_linearize ) { } /* ************************************************************************* */ -TEST( NonlinearConstraint, unary_scalar_equal ) { +TEST( NonlinearConstraint1, unary_scalar_equal ) { NonlinearConstraint1 c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1"), c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"), @@ -87,6 +84,99 @@ TEST( NonlinearConstraint, unary_scalar_equal ) { CHECK(!c1.equals(c4)); } +/* ************************************************************************* */ +// binary functions with scalar variables +/* ************************************************************************* */ +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); + 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); + 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); + return Vector_(1, x*x-5.0-y); +} +} // \namespace test2 + +/* ************************************************************************* */ +TEST( NonlinearConstraint2, binary_scalar_construction ) { + // 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; + NonlinearConstraint2 c1( + "x", *test2::grad_g1, + "y", *test2::grad_g2, + *test2::g_func, 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)); +} + +/* ************************************************************************* */ +TEST( NonlinearConstraint2, binary_scalar_linearize ) { + // create a constraint + size_t p = 1; + NonlinearConstraint2 c1( + "x", *test2::grad_g1, + "y", *test2::grad_g2, + *test2::g_func, p, "L_xy"); + + // get a configuration to use for finding the error + VectorConfig realconfig; + realconfig.insert("x", Vector_(1, 1.0)); + realconfig.insert("y", Vector_(1, 2.0)); + + // get a configuration of Lagrange multipliers + VectorConfig lagrangeConfig; + lagrangeConfig.insert("L_xy", Vector_(1, 3.0)); + + // linearize the system + GaussianFactor::shared_ptr actFactor, actConstraint; + boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig); + + // verify + GaussianFactor expFactor("x", Matrix_(1,1, 6.0), + "y", Matrix_(1,1, -3.0), + "L_xy", eye(1), zero(1), 1.0); + GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), + "y", Matrix_(1,1, -1.0), + Vector_(1,-6.0), 0.0); + CHECK(assert_equal(*actFactor, expFactor)); + CHECK(assert_equal(*actConstraint, expConstraint)); +} + +/* ************************************************************************* */ +TEST( NonlinearConstraint2, binary_scalar_equal ) { + NonlinearConstraint2 + c1("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"), + c2("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"), + c3("y", *test2::grad_g1, "x", *test2::grad_g2,*test2::g_func, 1, "L_xy"), + c4("x", *test2::grad_g1, "z", *test2::grad_g2,*test2::g_func, 3, "L_xy"); + + CHECK(assert_equal(c1, c2)); + CHECK(assert_equal(c2, c1)); + CHECK(!c1.equals(c3)); + CHECK(!c1.equals(c4)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */