From 67744a5f07ddf6327543b4a135dae73e84f51727 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 4 Feb 2010 16:08:11 +0000 Subject: [PATCH] Changed NonlinearConstraint to correctly use new keys --- cpp/NonlinearConstraint-inl.h | 80 +++--- cpp/NonlinearConstraint.h | 57 ++--- cpp/testNonlinearConstraint.cpp | 437 ++++++++++++++++---------------- 3 files changed, 275 insertions(+), 299 deletions(-) diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 1af200d94..a6d64afd4 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -18,7 +18,7 @@ namespace gtsam { /* ************************************************************************* */ template -NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key, +NonlinearConstraint::NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, Vector (*g)(const Config& config), bool isEquality) @@ -28,7 +28,7 @@ NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key /* ************************************************************************* */ template -NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key, +NonlinearConstraint::NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, boost::function g, bool isEquality) @@ -52,16 +52,11 @@ NonlinearConstraint1::NonlinearConstraint1( const Key& key, Matrix (*gradG)(const Config& config), size_t dim_constraint, - const std::string& lagrange_key, + const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G_(boost::bind(gradG, _1)), key_(key) { - // set a good lagrange key here - // TODO:should do something smart to find a unique one -// if (lagrange_key == "") -// this->lagrange_key_ = "L0" -// this->keys_.push_front(key); } /* ************************************************************************* */ @@ -71,16 +66,11 @@ NonlinearConstraint1::NonlinearConstraint1( const Key& key, boost::function gradG, size_t dim_constraint, - const std::string& lagrange_key, + const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G_(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); } /* ************************************************************************* */ @@ -102,17 +92,19 @@ bool NonlinearConstraint1::equals(const Factor& f, doubl 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 (!(this->lagrange_key_.equals(p->lagrange_key_))) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } /* ************************************************************************* */ template -std::pair -NonlinearConstraint1::linearize(const Config& config, const VectorConfig& lagrange) const { +GaussianFactor::shared_ptr +NonlinearConstraint1::linearize(const Config& config) const { + const size_t p = this->p_; + // extract lagrange multiplier - Vector lambda = lagrange[this->lagrange_key_]; + Vector lambda = config[this->lagrange_key_]; // find the error Vector g = g_(config); @@ -120,17 +112,25 @@ NonlinearConstraint1::linearize(const Config& config, const Vect // construct the gradient Matrix grad = G_(config); - // construct probabilistic factor - Matrix A1 = vector_scale(lambda, grad); - SharedDiagonal probModel = sharedSigma(this->p_,1.0); + // construct combined factor + Matrix Ax = zeros(grad.size1()*2, grad.size2()); + insertSub(Ax, vector_scale(lambda, grad), 0, 0); + insertSub(Ax, grad, grad.size1(), 0); + + Matrix AL = eye(p*2, p); + + Vector rhs = zero(p*2); + subInsert(rhs, -1*g, p); + + // construct a mixed constraint model + Vector sigmas = zero(p*2); + subInsert(sigmas, ones(p), 0); + SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas); + GaussianFactor::shared_ptr factor(new - GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); + GaussianFactor(key_, Ax, this->lagrange_key_, AL, rhs, mixedConstraint)); - // construct the constraint - SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); - GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel)); - - return std::make_pair(factor, constraint); + return factor; } /* ************************************************************************* */ @@ -146,18 +146,12 @@ NonlinearConstraint2::NonlinearConstraint2( const Key2& key2, Matrix (*G2)(const Config& config), size_t dim_constraint, - const std::string& lagrange_key, + const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), 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); } /* ************************************************************************* */ @@ -169,18 +163,12 @@ NonlinearConstraint2::NonlinearConstraint2( const Key2& key2, boost::function G2, size_t dim_constraint, - const std::string& lagrange_key, + const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G1_(G1), G2_(G2), 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); } /* ************************************************************************* */ @@ -200,17 +188,17 @@ bool NonlinearConstraint2::equals(const Factorkey1_)) return false; if (!(key2_ == p->key2_)) return false; - if (this->lagrange_key_ != p->lagrange_key_) return false; + if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } /* ************************************************************************* */ template -std::pair NonlinearConstraint2< - Config, Key1, X1, Key2, X2>::linearize(const Config& config, const VectorConfig& lagrange) const { +GaussianFactor::shared_ptr +NonlinearConstraint2::linearize(const Config& config) const { // extract lagrange multiplier - Vector lambda = lagrange[this->lagrange_key_]; + Vector lambda = config[this->lagrange_key_]; // find the error Vector g = g_(config); @@ -231,7 +219,7 @@ std::pair NonlinearConst GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1.0 * g, constraintModel)); - return std::make_pair(factor, constraint); + return factor; } } diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 7b65eb621..c5927df30 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -13,6 +13,9 @@ namespace gtsam { +/** Typedef for Lagrange key type - must be present in factors and config */ +typedef TypedSymbol LagrangeKey; + /** * Base class for nonlinear constraints * This allows for both equality and inequality constraints, @@ -29,7 +32,7 @@ class NonlinearConstraint : public NonlinearFactor { protected: /** key for the lagrange multipliers */ - std::string lagrange_key_; + LagrangeKey lagrange_key_; /** number of lagrange multipliers */ size_t p_; @@ -53,7 +56,7 @@ public: * @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, + NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, Vector (*g)(const Config& config), bool isEquality=true); @@ -64,13 +67,13 @@ public: * @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, + NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, boost::function g, bool isEquality=true); /** returns the key used for the Lagrange multipliers */ - std::string lagrangeKey() const { return lagrange_key_; } + LagrangeKey lagrangeKey() const { return lagrange_key_; } /** returns the number of lagrange multipliers */ size_t nrConstraints() const { return p_; } @@ -95,23 +98,11 @@ public: bool active(const Config& config) const; /** - * 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) + * Real linearize, given a config that includes Lagrange multipliers + * @param config is the configuration (with lagrange multipliers) + * @return a combined linear factor containing both the constraint and the constraint factor */ - virtual std::pair - linearize(const Config& config, const VectorConfig& lagrange) const=0; - - /** - * linearize with only Config, which is not currently implemented - * This will be implemented later for other constrained optimization - * algorithms - */ - virtual boost::shared_ptr linearize(const Config& c) const { - throw std::invalid_argument("No current constraint linearization for a single Config!"); - } + virtual boost::shared_ptr linearize(const Config& c) const=0; }; @@ -151,7 +142,7 @@ public: const Key& key, Matrix (*G)(const Config& config), size_t dim_constraint, - const std::string& lagrange_key="", + const LagrangeKey& lagrange_key, bool isEquality=true); /** @@ -168,7 +159,7 @@ public: const Key& key, boost::function G, size_t dim_constraint, - const std::string& lagrange_key="", + const LagrangeKey& lagrange_key, bool isEquality=true); /** Print */ @@ -178,14 +169,9 @@ public: bool equals(const Factor& f, double tol=1e-9) const; /** - * 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) + * Linearize from config - must have Lagrange multipliers */ - std::pair - linearize(const Config& config, const VectorConfig& lagrange) const; + virtual boost::shared_ptr linearize(const Config& c) const; }; /** @@ -228,7 +214,7 @@ public: const Key2& key2, Matrix (*G2)(const Config& config), size_t dim_constraint, - const std::string& lagrange_key="", + const LagrangeKey& lagrange_key, bool isEquality=true); /** @@ -248,7 +234,7 @@ public: const Key2& key2, boost::function G2, size_t dim_constraint, - const std::string& lagrange_key="", + const LagrangeKey& lagrange_key, bool isEquality=true); /** Print */ @@ -258,14 +244,9 @@ public: bool equals(const Factor& f, double tol=1e-9) const; /** - * 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) + * Linearize from config - must have Lagrange multipliers */ - std::pair - linearize(const Config& config, const VectorConfig& lagrange) const; + virtual boost::shared_ptr linearize(const Config& c) const; }; } diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 68928e2bd..33f6afeaf 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -23,7 +23,6 @@ typedef TypedSymbol Key; typedef NonlinearConstraint1 NLC1; typedef NonlinearConstraint2 NLC2; - /* ************************************************************************* */ // unary functions with scalar variables /* ************************************************************************* */ @@ -51,9 +50,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { size_t p = 1; list keys; keys += "x1"; Key x1(1); + LagrangeKey L1(1); NLC1 c1(boost::bind(test1::g, _1, keys), x1, boost::bind(test1::G, _1, keys), - p, "L1"); + p, L1); // get a configuration to use for finding the error VectorConfig config; @@ -70,40 +70,41 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { size_t p = 1; list keys; keys += "x1"; Key x1(1); + LagrangeKey L1(1); NLC1 c1(boost::bind(test1::g, _1, keys), x1, boost::bind(test1::G, _1, keys), - p, "L1"); + p, L1); - // get a configuration to use for linearization + // get a configuration to use for linearization (with lagrange multipliers) VectorConfig realconfig; realconfig.insert(x1, Vector_(1, 1.0)); - - // get a configuration of Lagrange multipliers - VectorConfig lagrangeConfig; - lagrangeConfig.insert("L1", Vector_(1, 3.0)); + realconfig.insert(L1, Vector_(1, 3.0)); // linearize the system - GaussianFactor::shared_ptr actualFactor, actualConstraint; - boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); + GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig); - // verify - SharedDiagonal probModel = sharedSigma(p,1.0); - GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); - SharedDiagonal constraintModel = noiseModel::Constrained::All(p); - GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); - CHECK(assert_equal(*actualFactor, expectedFactor)); - CHECK(assert_equal(*actualConstraint, expectedConstraint)); + // verify - probabilistic component goes on top + Vector sigmas = Vector_(2, 1.0, 0.0); + SharedDiagonal mixedModel = noiseModel::Constrained::MixedSigmas(sigmas); + // stack the matrices to combine + Matrix Ax1 = Matrix_(2,1, 6.0, 2.0), + AL1 = Matrix_(2,1, 1.0, 0.0); + Vector rhs = Vector_(2, 0.0, 4.0); + GaussianFactor expectedFactor(x1, Ax1, L1, AL1, rhs, mixedModel); + + CHECK(assert_equal(*linfactor, expectedFactor)); } /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_equal ) { list keys1, keys2; keys1 += "x0"; keys2 += "x1"; Key x(0), y(1); + LagrangeKey L1(1); NLC1 - c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1", true), - c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1"), - c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, "L_x1"), - c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, "L_x1"); + c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true), + c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1), + c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, L1), + c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, L1); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -145,11 +146,12 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { size_t p = 1; list keys; keys += "x0", "x1"; Key x0(0), x1(1); + LagrangeKey L1(1); NLC2 c1( boost::bind(test2::g, _1, keys), x0, boost::bind(test2::G1, _1, keys), x1, boost::bind(test2::G1, _1, keys), - p, "L12"); + p, L1); // get a configuration to use for finding the error VectorConfig config; @@ -168,36 +170,40 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { size_t p = 1; list keys; keys += "x0", "x1"; Key x0(0), x1(1); + LagrangeKey L1(1); NLC2 c1( boost::bind(test2::g, _1, keys), x0, boost::bind(test2::G1, _1, keys), x1, boost::bind(test2::G2, _1, keys), - p, "L12"); + p, L1); // get a configuration to use for finding the error VectorConfig realconfig; realconfig.insert(x0, Vector_(1, 1.0)); realconfig.insert(x1, Vector_(1, 2.0)); - - // get a configuration of Lagrange multipliers - VectorConfig lagrangeConfig; - lagrangeConfig.insert("L12", Vector_(1, 3.0)); + realconfig.insert(L1, Vector_(1, 3.0)); // linearize the system - GaussianFactor::shared_ptr actualFactor, actualConstraint; - boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); + GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig); // verify - SharedDiagonal probModel = sharedSigma(p,1.0); - GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), - x1, Matrix_(1,1, -3.0), - "L12", eye(1), zero(1), probModel); - SharedDiagonal constraintModel = noiseModel::Constrained::All(p); - GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), - x1, Matrix_(1,1, -1.0), - Vector_(1, 6.0), constraintModel); - CHECK(assert_equal(*actualFactor, expectedFactor)); - CHECK(assert_equal(*actualConstraint, expectedConstraint)); + Matrix Ax0 = Matrix_(2,1, 6.0, 2.0), + Ax1 = Matrix_(2,1,-3.0,-1.0), + AL = Matrix_(2,1, 1.0, 0.0); + Vector rhs = Vector_(2, 0, 6.0), + sigmas = Vector_(2, 1.0, 0.0); + SharedDiagonal expModel = noiseModel::Constrained::MixedSigmas(sigmas); + +// SharedDiagonal probModel = sharedSigma(p,1.0); +// GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), +// x1, Matrix_(1,1, -3.0), +// L1, eye(1), zero(1), probModel); +// SharedDiagonal constraintModel = noiseModel::Constrained::All(p); +// GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), +// x1, Matrix_(1,1, -1.0), +// Vector_(1, 6.0), constraintModel); +// CHECK(assert_equal(*actualFactor, expectedFactor)); +// CHECK(assert_equal(*actualConstraint, expectedConstraint)); } /* ************************************************************************* */ @@ -205,11 +211,12 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) { list keys1, keys2, keys3; keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z"; Key x0(0), x1(1), x2(2); + LagrangeKey L1(1); NLC2 - c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"), - c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"), - c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, "L_xy"), - c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, "L_xy"); + c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1), + c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1), + c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, L1), + c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, L1); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -217,178 +224,178 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) { CHECK(!c1.equals(c4)); } -/* ************************************************************************* */ -// Inequality tests -/* ************************************************************************* */ -namespace inequality1 { - - /** p = 1, g(x) x^2 - 5 > 0 */ - Vector g(const VectorConfig& config, const Key& key) { - double x = config[key](0); - double g = x * x - 5; - return Vector_(1, g); // return the actual cost - } - - /** p = 1, jacobianG(x) = 2*x */ - Matrix G(const VectorConfig& config, const Key& key) { - double x = config[key](0); - return Matrix_(1, 1, 2 * x); - } - -} // \namespace inequality1 - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_inequality ) { - size_t p = 1; - Key x0(0); - NLC1 c1(boost::bind(inequality1::g, _1, x0), - x0, boost::bind(inequality1::G, _1, x0), - p, "L1", - false); // inequality constraint - - // get configurations to use for evaluation - VectorConfig config1, config2; - config1.insert(x0, Vector_(1, 10.0)); // should be inactive - config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error - - // check error - CHECK(!c1.active(config1)); - Vector actualError2 = c1.unwhitenedError(config2); - CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); - CHECK(c1.active(config2)); -} - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_inequality_linearize ) { - size_t p = 1; - Key x0(0); - NLC1 c1(boost::bind(inequality1::g, _1, x0), - x0, boost::bind(inequality1::G, _1, x0), - p, "L1", - false); // inequality constraint - - // get configurations to use for linearization - VectorConfig config1, config2; - config1.insert(x0, Vector_(1, 10.0)); // should have zero error - config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error - - // get a configuration of Lagrange multipliers - VectorConfig lagrangeConfig; - lagrangeConfig.insert("L1", Vector_(1, 3.0)); - - // linearize for inactive constraint - GaussianFactor::shared_ptr actualFactor1, actualConstraint1; - boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig); - - // check if the factor is active - CHECK(!c1.active(config1)); - - // linearize for active constraint - GaussianFactor::shared_ptr actualFactor2, actualConstraint2; - boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig); - CHECK(c1.active(config2)); - - // verify - SharedDiagonal probModel = sharedSigma(p,1.0); - GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); - SharedDiagonal constraintModel = noiseModel::Constrained::All(p); - GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); - CHECK(assert_equal(*actualFactor2, expectedFactor)); - CHECK(assert_equal(*actualConstraint2, expectedConstraint)); -} - -/* ************************************************************************* */ -// Binding arbitrary functions -/* ************************************************************************* */ -namespace binding1 { - - /** p = 1, g(x) x^2 - r > 0 */ - Vector g(double r, const VectorConfig& config, const Key& key) { - double x = config[key](0); - double g = x * x - r; - return Vector_(1, g); // return the actual cost - } - - /** p = 1, jacobianG(x) = 2*x */ - Matrix G(double coeff, const VectorConfig& config, - const Key& key) { - double x = config[key](0); - return Matrix_(1, 1, coeff * x); - } - -} // \namespace binding1 - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_binding ) { - size_t p = 1; - double coeff = 2; - double radius = 5; - Key x0(0); - NLC1 c1(boost::bind(binding1::g, radius, _1, x0), - x0, boost::bind(binding1::G, coeff, _1, x0), - p, "L1", - false); // inequality constraint - - // get configurations to use for evaluation - VectorConfig config1, config2; - config1.insert(x0, Vector_(1, 10.0)); // should have zero error - config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error - - // check error - CHECK(!c1.active(config1)); - Vector actualError2 = c1.unwhitenedError(config2); - CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); - CHECK(c1.active(config2)); -} - -/* ************************************************************************* */ -namespace binding2 { - - /** p = 1, g(x) = x^2-5 -y = 0 */ - Vector g(double r, const VectorConfig& config, const Key& k1, const Key& k2) { - double x = config[k1](0); - double y = config[k2](0); - return Vector_(1, x * x - r - y); - } - - /** jacobian for x, jacobianG(x,y) in x: 2x*/ - Matrix G1(double c, const VectorConfig& config, const Key& key) { - double x = config[key](0); - return Matrix_(1, 1, c * x); - } - - /** jacobian for y, jacobianG(x,y) in y: -1 */ - Matrix G2(double c, const VectorConfig& config) { - return Matrix_(1, 1, -1.0 * c); - } - -} // \namespace binding2 - -/* ************************************************************************* */ -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; - Key x0(0), x1(1); - NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1), - x0, boost::bind(binding2::G1, a, _1, x0), - x1, boost::bind(binding2::G2, b, _1), - p, "L1"); - - // get a configuration to use for finding the error - VectorConfig config; - config.insert(x0, Vector_(1, 1.0)); - config.insert(x1, Vector_(1, 2.0)); - - // calculate the error - Vector actual = c1.unwhitenedError(config); - Vector expected = Vector_(1.0, -6.0); - CHECK(assert_equal(actual, expected, 1e-5)); -} +///* ************************************************************************* */ +//// Inequality tests +///* ************************************************************************* */ +//namespace inequality1 { +// +// /** p = 1, g(x) x^2 - 5 > 0 */ +// Vector g(const VectorConfig& config, const Key& key) { +// double x = config[key](0); +// double g = x * x - 5; +// return Vector_(1, g); // return the actual cost +// } +// +// /** p = 1, jacobianG(x) = 2*x */ +// Matrix G(const VectorConfig& config, const Key& key) { +// double x = config[key](0); +// return Matrix_(1, 1, 2 * x); +// } +// +//} // \namespace inequality1 +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint1, unary_inequality ) { +// size_t p = 1; +// Key x0(0); +// NLC1 c1(boost::bind(inequality1::g, _1, x0), +// x0, boost::bind(inequality1::G, _1, x0), +// p, "L1", +// false); // inequality constraint +// +// // get configurations to use for evaluation +// VectorConfig config1, config2; +// config1.insert(x0, Vector_(1, 10.0)); // should be inactive +// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error +// +// // check error +// CHECK(!c1.active(config1)); +// Vector actualError2 = c1.unwhitenedError(config2); +// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); +// CHECK(c1.active(config2)); +//} +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint1, unary_inequality_linearize ) { +// size_t p = 1; +// Key x0(0); +// NLC1 c1(boost::bind(inequality1::g, _1, x0), +// x0, boost::bind(inequality1::G, _1, x0), +// p, "L1", +// false); // inequality constraint +// +// // get configurations to use for linearization +// VectorConfig config1, config2; +// config1.insert(x0, Vector_(1, 10.0)); // should have zero error +// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error +// +// // get a configuration of Lagrange multipliers +// VectorConfig lagrangeConfig; +// lagrangeConfig.insert("L1", Vector_(1, 3.0)); +// +// // linearize for inactive constraint +// GaussianFactor::shared_ptr actualFactor1, actualConstraint1; +// boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig); +// +// // check if the factor is active +// CHECK(!c1.active(config1)); +// +// // linearize for active constraint +// GaussianFactor::shared_ptr actualFactor2, actualConstraint2; +// boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig); +// CHECK(c1.active(config2)); +// +// // verify +// SharedDiagonal probModel = sharedSigma(p,1.0); +// GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); +// SharedDiagonal constraintModel = noiseModel::Constrained::All(p); +// GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); +// CHECK(assert_equal(*actualFactor2, expectedFactor)); +// CHECK(assert_equal(*actualConstraint2, expectedConstraint)); +//} +// +///* ************************************************************************* */ +//// Binding arbitrary functions +///* ************************************************************************* */ +//namespace binding1 { +// +// /** p = 1, g(x) x^2 - r > 0 */ +// Vector g(double r, const VectorConfig& config, const Key& key) { +// double x = config[key](0); +// double g = x * x - r; +// return Vector_(1, g); // return the actual cost +// } +// +// /** p = 1, jacobianG(x) = 2*x */ +// Matrix G(double coeff, const VectorConfig& config, +// const Key& key) { +// double x = config[key](0); +// return Matrix_(1, 1, coeff * x); +// } +// +//} // \namespace binding1 +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint1, unary_binding ) { +// size_t p = 1; +// double coeff = 2; +// double radius = 5; +// Key x0(0); +// NLC1 c1(boost::bind(binding1::g, radius, _1, x0), +// x0, boost::bind(binding1::G, coeff, _1, x0), +// p, "L1", +// false); // inequality constraint +// +// // get configurations to use for evaluation +// VectorConfig config1, config2; +// config1.insert(x0, Vector_(1, 10.0)); // should have zero error +// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error +// +// // check error +// CHECK(!c1.active(config1)); +// Vector actualError2 = c1.unwhitenedError(config2); +// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); +// CHECK(c1.active(config2)); +//} +// +///* ************************************************************************* */ +//namespace binding2 { +// +// /** p = 1, g(x) = x^2-5 -y = 0 */ +// Vector g(double r, const VectorConfig& config, const Key& k1, const Key& k2) { +// double x = config[k1](0); +// double y = config[k2](0); +// return Vector_(1, x * x - r - y); +// } +// +// /** jacobian for x, jacobianG(x,y) in x: 2x*/ +// Matrix G1(double c, const VectorConfig& config, const Key& key) { +// double x = config[key](0); +// return Matrix_(1, 1, c * x); +// } +// +// /** jacobian for y, jacobianG(x,y) in y: -1 */ +// Matrix G2(double c, const VectorConfig& config) { +// return Matrix_(1, 1, -1.0 * c); +// } +// +//} // \namespace binding2 +// +///* ************************************************************************* */ +//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; +// Key x0(0), x1(1); +// NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1), +// x0, boost::bind(binding2::G1, a, _1, x0), +// x1, boost::bind(binding2::G2, b, _1), +// p, "L1"); +// +// // get a configuration to use for finding the error +// VectorConfig config; +// config.insert(x0, Vector_(1, 1.0)); +// config.insert(x1, Vector_(1, 2.0)); +// +// // calculate the error +// Vector actual = c1.unwhitenedError(config); +// Vector expected = Vector_(1.0, -6.0); +// CHECK(assert_equal(actual, expected, 1e-5)); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }