From ee4a06627555536053746e87b638a5223e9c6953 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 20 Nov 2009 03:04:49 +0000 Subject: [PATCH] Implemented linearization and equality for unary NonlinearConstraints. Current tests use a scalar example. Split out implementation into a separate implementation file. --- cpp/Makefile.am | 2 +- cpp/NonlinearConstraint-inl.h | 60 +++++++++++++++++++++++++ cpp/NonlinearConstraint.h | 23 +++------- cpp/testNonlinearConstraint.cpp | 80 +++++++++++++++++---------------- cpp/testSQP.cpp | 2 +- 5 files changed, 111 insertions(+), 56 deletions(-) create mode 100644 cpp/NonlinearConstraint-inl.h diff --git a/cpp/Makefile.am b/cpp/Makefile.am index da712d7d9..862a1b1ab 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -115,7 +115,7 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h sources += NonlinearFactor.cpp sources += NonlinearEquality.h -sources += NonlinearConstraint.h +sources += NonlinearConstraint.h NonlinearConstraint-inl.h check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h new file mode 100644 index 000000000..46f509f8a --- /dev/null +++ b/cpp/NonlinearConstraint-inl.h @@ -0,0 +1,60 @@ +/* + * @file NonlinearConstraint-inl.h + * @brief Implementation for NonlinearConstraints + * @author alexgc + */ + +#pragma once + +#include +#include "NonlinearConstraint.h" + +namespace gtsam { + +/** + * Implementations of unary nonlinear constraints + */ + +template +void NonlinearConstraint1::print(const std::string& s) const { + std::cout << "NonlinearConstraint [" << s << "]:\n" + << " Variable: " << 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); + 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 (gradG_ != p->gradG_) return false; + return this->p_ == p->p_; +} + +template +std::pair +NonlinearConstraint1::linearize(const Config& config, const VectorConfig& lagrange) const { + // extract lagrange multiplier + Vector lambda = lagrange[this->lagrange_key_]; + + // find the error + Vector g = g_(config, key_); + + // construct the gradient + Matrix grad = gradG_(config, key_); + + // construct probabilistic factor + Matrix A1 = vector_scale(grad, lambda); + GaussianFactor::shared_ptr factor(new + 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)); + + return std::make_pair(factor, constraint); +} + +} diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 0325e96c7..dcc7ac8bc 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include "NonlinearFactor.h" namespace gtsam { @@ -81,7 +82,7 @@ public: * A unary constraint with arbitrary cost and gradient functions */ template -class NonlinearConstraint1 : NonlinearConstraint { +class NonlinearConstraint1 : public NonlinearConstraint { private: /** calculates the constraint function of the current config @@ -122,21 +123,17 @@ public: const std::string& lagrange_key="") : NonlinearConstraint(lagrange_key, dim_constraint), g_(g), gradG_(gradG), key_(key) { - // set a good lagrange key here - should do something smart to find a unique one + // set a good lagrange key here + // TODO:should do something smart to find a unique one if (lagrange_key == "") this->lagrange_key_ = "L_" + key; } /** Print */ - void print(const std::string& s = "") const { - //FIXME: dummy implementation - } + void print(const std::string& s = "") const; /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const { - //FIXME: dummy implementation - return false; - } + 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 { @@ -151,13 +148,7 @@ public: * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint) */ std::pair - linearize(const Config& config, const VectorConfig& lagrange) const { - //FIXME: dummy implementation - GaussianFactor::shared_ptr factor(new GaussianFactor); - GaussianFactor::shared_ptr constraint(new GaussianFactor); - return std::make_pair(factor, constraint); - } - + linearize(const Config& config, const VectorConfig& lagrange) const; }; diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index bffbd7f55..32ac50a83 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -7,14 +7,12 @@ #include #include #include +#include using namespace gtsam; -// define some functions to use -// these examples use scalar variables and a single constraint - /* ************************************************************************* */ -// unary functions +// unary functions with scalar variables /* ************************************************************************* */ namespace test1 { /** p = 1, gradG(x) = 2x */ @@ -31,7 +29,7 @@ Vector g_func(const VectorConfig& config, const std::string& key) { } // \namespace test1 /* ************************************************************************* */ -TEST( NonlinearConstraint, unary_construction ) { +TEST( NonlinearConstraint, unary_scalar_construction ) { // construct a constraint on x // the lagrange multipliers will be expected on L_x1 // and there is only one multiplier @@ -48,40 +46,46 @@ TEST( NonlinearConstraint, unary_construction ) { CHECK(assert_equal(actual, expected, 1e-5)); } -///* ************************************************************************* */ -//TEST( NonlinearConstraint, unary_linearize ) { -// // constuct a constraint -// Vector lambda = Vector_(1, 1.0); -// NonlinearConstraint1 c1("x", *grad_g1, *g1_func, lambda); -// -// // get a configuration to use for linearization -// VectorConfig config; -// config.insert("x", Vector_(1, 1.0)); -// -// // determine the gradient of the function -// Matrix -// -//} +/* ************************************************************************* */ +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 + size_t p = 1; + NonlinearConstraint1 c1("x", *test1::grad_g, *test1::g_func, p, "L_x1"); -///* ************************************************************************* */ -//// binary functions -//Matrix grad1_g2(const VectorConfig& config) { -// return eye(1); -//} -// -//Matrix grad2_g2(const VectorConfig& config) { -// return eye(1); -//} -// -//Vector g2_func(const VectorConfig& config) { -// return zero(1); -//} -// -///* ************************************************************************* */ -//TEST( NonlinearConstraint, binary_construction ) { -// Vector lambda = Vector_(1, 1.0); -// NonlinearConstraint2 c1("x", *grad1_g2, "y", *grad2_g2, *g2_func, lambda); -//} + // get a configuration to use for linearization + VectorConfig realconfig; + realconfig.insert("x", Vector_(1, 1.0)); + + // get a configuration of Lagrange multipliers + VectorConfig lagrangeConfig; + lagrangeConfig.insert("L_x1", 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), "L_x1", eye(1), zero(1), 1.0); + GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1,-4.0), 0.0); + CHECK(assert_equal(*actFactor, expFactor)); + CHECK(assert_equal(*actConstraint, expConstraint)); +} + +/* ************************************************************************* */ +TEST( NonlinearConstraint, 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"), + c3("x", *test1::grad_g, *test1::g_func, 2, "L_x1"), + c4("y", *test1::grad_g, *test1::g_func, 1, "L_x1"); + + 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); } diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 2156380e2..c559921b5 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -192,7 +192,7 @@ TEST (SQP, problem1_sqp ) { * to zero * lam*gradG*dx + dlam + lam * formulated in matrix form as: - * [lam*gradG eye(1)] [dx; dlam] = lam + * [lam*gradG eye(1)] [dx; dlam] = zero */ GaussianFactor::shared_ptr f2( new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)