From a55860eb1d0f9e1a499377a04a475d3a0bcc3804 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 6 Aug 2010 18:30:07 +0000 Subject: [PATCH] Reimplemented nonlinear constraints to remove the old dependency on using boost function pointers and allow for inequality and bounding constraints. Added simple equality constraints in simulated2D and new set of tests. Removed/disabled old tests until they can be worked into new structure. --- nonlinear/Makefile.am | 3 +- nonlinear/NonlinearConstraint-inl.h | 166 -- nonlinear/NonlinearConstraint.h | 225 +-- nonlinear/NonlinearFactor.h | 4 - nonlinear/tests/testConstraintOptimizer.cpp | 100 + nonlinear/tests/testNonlinearConstraint.cpp | 373 ---- slam/simulated2D.h | 4 +- slam/simulated2DConstraints.h | 67 + tests/Makefile.am | 4 +- tests/testNonlinearConstraint.cpp | 1888 ++++++++----------- tests/testNonlinearEqualityConstraint.cpp | 191 ++ 11 files changed, 1314 insertions(+), 1711 deletions(-) delete mode 100644 nonlinear/NonlinearConstraint-inl.h delete mode 100644 nonlinear/tests/testNonlinearConstraint.cpp create mode 100644 slam/simulated2DConstraints.h create mode 100644 tests/testNonlinearEqualityConstraint.cpp diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index 8788cd3a0..5ae04a057 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -22,9 +22,8 @@ headers += NonlinearFactor.h sources += NonlinearOptimizer.cpp # Nonlinear constraints -headers += NonlinearConstraint.h NonlinearConstraint-inl.h +headers += NonlinearConstraint.h headers += NonlinearEquality.h -check_PROGRAMS += tests/testNonlinearConstraint # SQP if USE_LDL diff --git a/nonlinear/NonlinearConstraint-inl.h b/nonlinear/NonlinearConstraint-inl.h deleted file mode 100644 index 3fe0f9e90..000000000 --- a/nonlinear/NonlinearConstraint-inl.h +++ /dev/null @@ -1,166 +0,0 @@ -/* - * @file NonlinearConstraint-inl.h - * @brief Implementation for NonlinearConstraints - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#include "NonlinearConstraint.h" - -#define INSTANTIATE_NONLINEAR_CONSTRAINT(C) \ - INSTANTIATE_FACTOR_GRAPH(NonlinearConstraint); \ - template class NonlinearConstraint; - -namespace gtsam { - -/* ************************************************************************* */ -// Implementations of base class -/* ************************************************************************* */ - -/* ************************************************************************* */ -template -NonlinearConstraint::NonlinearConstraint(size_t dim, double mu) : - NonlinearFactor(noiseModel::Constrained::All(dim)), - mu_(fabs(mu)) -{ -} - -/* ************************************************************************* */ -template -double NonlinearConstraint::error(const Config& c) const { - const Vector error_vector = unwhitenedError(c); - return mu_ * inner_prod(error_vector, error_vector); -} - -/* ************************************************************************* */ -// Implementations of unary nonlinear constraints -/* ************************************************************************* */ - -template -NonlinearConstraint1::NonlinearConstraint1( - Vector (*g)(const Config& config), - const Key& key, - Matrix (*gradG)(const Config& config), - size_t dim, double mu) : - NonlinearConstraint(dim, mu), - G_(boost::bind(gradG, _1)), g_(boost::bind(g, _1)), key_(key) -{ - this->keys_.push_back(key); -} - -/* ************************************************************************* */ -template -NonlinearConstraint1::NonlinearConstraint1( - boost::function g, - const Key& key, - boost::function gradG, - size_t dim, double mu) : - NonlinearConstraint(dim, mu), - G_(gradG), g_(g), key_(key) -{ - this->keys_.push_back(key); -} - -/* ************************************************************************* */ -template -void NonlinearConstraint1::print(const std::string& s) const { - std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->dim() - << " mu: " << this->mu_ << "\n" - << " Key : " << (std::string) this->key_ << "\n"; -} - -/* ************************************************************************* */ -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 (fabs(this->mu_ - p->mu_ ) > tol) return false; - return this->dim() == p->dim(); -} - -/* ************************************************************************* */ -template -GaussianFactor::shared_ptr -NonlinearConstraint1::linearize(const Config& config) const { - Vector g = -1.0 * g_(config); - Matrix grad = G_(config); - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - GaussianFactor::shared_ptr factor(new GaussianFactor(this->key_, grad, g, model)); - return factor; -} - -/* ************************************************************************* */ -// Implementations of binary nonlinear constraints -/* ************************************************************************* */ - -/* ************************************************************************* */ -template -NonlinearConstraint2::NonlinearConstraint2( - Vector (*g)(const Config& config), - const Key1& key1, - Matrix (*G1)(const Config& config), - const Key2& key2, - Matrix (*G2)(const Config& config), - size_t dim, double mu) - : NonlinearConstraint(dim, mu), - G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)), - key1_(key1), key2_(key2) -{ - this->keys_.push_back(key1); - this->keys_.push_back(key2); -} - -/* ************************************************************************* */ -template -NonlinearConstraint2::NonlinearConstraint2( - boost::function g, - const Key1& key1, - boost::function G1, - const Key2& key2, - boost::function G2, - size_t dim, double mu) - : NonlinearConstraint(dim, mu), - G1_(G1), G2_(G2), g_(g), - key1_(key1), key2_(key2) -{ - this->keys_.push_back(key1); - this->keys_.push_back(key2); -} - -/* ************************************************************************* */ -template -void NonlinearConstraint2::print(const std::string& s) const { - std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->dim() - << " mu: " << this->mu_ << "\n" - << " Key1 : " << (std::string) this->key1_ << "\n" - << " Key2 : " << (std::string) this->key2_ << "\n"; -} - -/* ************************************************************************* */ -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 (fabs(this->mu_ - p->mu_ ) > tol) return false; - return this->dim() == p->dim(); -} - -/* ************************************************************************* */ -template -GaussianFactor::shared_ptr -NonlinearConstraint2::linearize(const Config& config) const { - Vector g = -1.0 * g_(config); - Matrix grad1 = G1_(config); - Matrix grad2 = G2_(config); - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - GaussianFactor::shared_ptr factor(new GaussianFactor(this->key1_, grad1, this->key2_, grad2, g, model)); - return factor; -} - -} diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index e9fc1f89f..d1260e2b0 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -31,7 +31,8 @@ template class NonlinearConstraint : public NonlinearFactor { protected: - + typedef NonlinearConstraint This; + typedef NonlinearFactor Base; double mu_; // gain for quadratic merit function public: @@ -40,7 +41,8 @@ public: * @param dim is the dimension of the factor * @param mu is the gain used at error evaluation (forced to be positive) */ - NonlinearConstraint(size_t dim, double mu = 1000.0); + NonlinearConstraint(size_t dim, double mu = 1000.0): + Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {} /** returns the gain mu */ double mu() const { return mu_; } @@ -49,10 +51,29 @@ public: virtual void print(const std::string& s = "") const=0; /** Check if two factors are equal */ - virtual bool equals(const Factor& f, double tol=1e-9) const=0; + virtual bool equals(const Factor& f, double tol=1e-9) const { + const This* p = dynamic_cast (&f); + if (p == NULL) return false; + return Base::equals(*p, tol) && (mu_ == p->mu_); + } /** error function - returns the quadratic merit function */ - virtual double error(const Config& c) const; + virtual double error(const Config& c) const { + const Vector error_vector = unwhitenedError(c); + if (active(c)) + return mu_ * inner_prod(error_vector, error_vector); + else return 0.0; + } + + /** Raw error vector function g(x) */ + virtual Vector unwhitenedError(const Config& c) const = 0; + + /** + * active set check, defines what type of constraint this is + * By default, the constraint is always active, so it is an equality constraint + * @return true if the constraint is active + */ + virtual bool active(const Config& c) const { return true; } /** * Linearizes around a given config @@ -64,31 +85,14 @@ public: /** - * A unary constraint with arbitrary cost and jacobian functions - * This is an example class designed for easy testing, but real uses should probably - * subclass NonlinearConstraint and implement virtual functions directly + * A unary constraint that defaults to an equality constraint */ template class NonlinearConstraint1 : public NonlinearConstraint { -private: - - /** - * Calculates the jacobian of the constraint function - * returns a pxn matrix - * Use boost.bind to create the function object - * @param config to use for linearization - * @return the jacobian of the constraint in terms of key - */ - boost::function G_; - - /** 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 - * @return the cost for each of p constraints, arranged in a vector - */ - boost::function g_; +protected: + typedef NonlinearConstraint1 This; + typedef NonlinearConstraint Base; /** key for the constrained variable */ Key key_; @@ -98,44 +102,52 @@ public: /** * Basic constructor * @param key is the identifier for the variable constrained - * @param G gives the jacobian of the constraint function - * @param g is the constraint function * @param dim is the size of the constraint (p) * @param mu is the gain for the factor */ - NonlinearConstraint1( - Vector (*g)(const Config& config), - const Key& key, - Matrix (*G)(const Config& config), - size_t dim, - double mu = 1000.0); + NonlinearConstraint1(const Key& key, size_t dim, double mu = 1000.0) + : Base(dim, mu), key_(key) { + this->keys_.push_back(key); + } - /** - * Basic constructor with boost function pointers - * @param key is the identifier for the variable constrained - * @param G gives the jacobian of the constraint function - * @param g is the constraint function as a boost function pointer - * @param dim is the size of the constraint (p) - * @param mu is the gain for the factor - */ - NonlinearConstraint1( - boost::function g, - const Key& key, - boost::function G, - size_t dim, - double mu = 1000.0); + /* print */ + void print(const std::string& s = "") const { + std::cout << "NonlinearConstraint1 " << s << std::endl; + std::cout << "key: " << (std::string) key_ << std::endl; + std::cout << "mu: " << this->mu_ << std::endl; + } - /** Print */ - void print(const std::string& s = "") const; + /** Check if two factors are equal. Note type is Factor and needs cast. */ + virtual bool equals(const Factor& f, double tol = 1e-9) const { + const This* p = dynamic_cast (&f); + if (p == NULL) return false; + return Base::equals(*p, tol) && (key_ == p->key_); + } - /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const; - - /** Error function */ - virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); } + /** error function g(x) */ + inline Vector unwhitenedError(const Config& x) const { + const Key& j = key_; + const X& xj = x[j]; + return evaluateError(xj); + } /** Linearize from config */ - virtual boost::shared_ptr linearize(const Config& c) const; + boost::shared_ptr linearize(const Config& c) const { + if (!active(c)) { + boost::shared_ptr factor; + return factor; + } + const Key& j = key_; + const X& x = c[j]; + Matrix grad; + Vector g = -1.0 * evaluateError(x, grad); + SharedDiagonal model = noiseModel::Constrained::All(this->dim()); + return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, grad, g, model)); + } + + /** g(x) with optional derivative */ + virtual Vector evaluateError(const X& x, boost::optional H = + boost::none) const = 0; }; /** @@ -144,25 +156,9 @@ public: template class NonlinearConstraint2 : public NonlinearConstraint { -private: - - /** - * Calculates the jacobians of the constraint function in terms of - * the first and second variables - * returns a pxn matrix - * @param config to use for linearization - * @return the jacobian of the constraint in terms of key - */ - boost::function G1_; - boost::function G2_; - - /** 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 - * @return the cost for each of p constraints, arranged in a vector - */ - boost::function g_; +protected: + typedef NonlinearConstraint2 This; + typedef NonlinearConstraint Base; /** keys for the constrained variables */ Key1 key1_; @@ -172,50 +168,59 @@ public: /** * Basic constructor - * @param key is the identifier for the variable constrained - * @param G gives the jacobian of the constraint function - * @param g is the constraint function + * @param key1 is the identifier for the first variable constrained + * @param key2 is the identifier for the second variable constrained * @param dim is the size of the constraint (p) * @param mu is the gain for the factor */ - NonlinearConstraint2( - Vector (*g)(const Config& config), - const Key1& key1, - Matrix (*G1)(const Config& config), - const Key2& key2, - Matrix (*G2)(const Config& config), - size_t dim, - double mu = 1000.0); + NonlinearConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) : + Base(dim, mu), key1_(key1), key2_(key2) { + this->keys_.push_back(key1); + this->keys_.push_back(key2); + } - /** - * Basic constructor with direct function objects - * Use boost.bind to construct the function objects - * @param key is the identifier for the variable constrained - * @param G gives the jacobian of the constraint function - * @param g is the constraint function - * @param dim is the size of the constraint (p) - * @param mu is the gain for the factor - */ - NonlinearConstraint2( - boost::function g, - const Key1& key1, - boost::function G1, - const Key2& key2, - boost::function G2, - size_t dim, - double mu = 1000.0); + /* print */ + void print(const std::string& s = "") const { + std::cout << "NonlinearConstraint2 " << s << std::endl; + std::cout << "key1: " << (std::string) key1_ << std::endl; + std::cout << "key2: " << (std::string) key2_ << std::endl; + std::cout << "mu: " << this->mu_ << std::endl; + } - /** Print */ - void print(const std::string& s = "") const; + /** Check if two factors are equal. Note type is Factor and needs cast. */ + virtual bool equals(const Factor& f, double tol = 1e-9) const { + const This* p = dynamic_cast (&f); + if (p == NULL) return false; + return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_); + } - /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const; - - /** Error function */ - virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); } + /** error function g(x) */ + inline Vector unwhitenedError(const Config& x) const { + const Key1& j1 = key1_; + const Key2& j2 = key2_; + const X1& xj1 = x[j1]; + const X2& xj2 = x[j2]; + return evaluateError(xj1, xj2); + } /** Linearize from config */ - virtual boost::shared_ptr linearize(const Config& c) const; + boost::shared_ptr linearize(const Config& c) const { + if (!active(c)) { + boost::shared_ptr factor; + return factor; + } + const Key1& j1 = key1_; const Key2& j2 = key2_; + const X1& x1 = c[j1]; const X2& x2 = c[j2]; + Matrix grad1, grad2; + Vector g = -1.0 * evaluateError(x1, x2, grad1, grad2); + SharedDiagonal model = noiseModel::Constrained::All(this->dim()); + return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, g, model)); + } + + /** g(x) with optional derivative2 */ + virtual Vector evaluateError(const X1& x1, const X2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const = 0; }; } diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index a79ef33c6..cc7cf745f 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -236,8 +236,6 @@ namespace gtsam { /** * A Gaussian nonlinear factor that takes 2 parameters - * Note: cannot be serialized as contains function pointers - * Specialized derived classes could do this */ template class NonlinearFactor2: public NonlinearFactor { @@ -353,8 +351,6 @@ namespace gtsam { /** * A Gaussian nonlinear factor that takes 3 parameters - * Note: cannot be serialized as contains function pointers - * Specialized derived classes could do this */ template class NonlinearFactor3: public NonlinearFactor { diff --git a/nonlinear/tests/testConstraintOptimizer.cpp b/nonlinear/tests/testConstraintOptimizer.cpp index 3eabab300..c44e12d8c 100644 --- a/nonlinear/tests/testConstraintOptimizer.cpp +++ b/nonlinear/tests/testConstraintOptimizer.cpp @@ -25,6 +25,106 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +/* ********************************************************************* + * Example from SQP testing: + * + * This example uses a nonlinear objective function and + * nonlinear equality constraint. The formulation is actually + * the Cholesky form that creates the full Hessian explicitly, + * and isn't expecially compatible with our machinery. + */ +TEST (NonlinearConstraint, problem1_cholesky ) { + bool verbose = false; + // use a nonlinear function of f(x) = x^2+y^2 + // nonlinear equality constraint: g(x) = x^2-5-y=0 + // Lagrangian: f(x) + \lambda*g(x) + + // Symbols + Symbol x1("x1"), y1("y1"), L1("L1"); + + // state structure: [x y \lambda] + VectorConfig init, state; + init.insert(x1, Vector_(1, 1.0)); + init.insert(y1, Vector_(1, 1.0)); + init.insert(L1, Vector_(1, 1.0)); + state = init; + + if (verbose) init.print("Initial State"); + + // loop until convergence + int maxIt = 10; + for (int i = 0; i -#include -#include -#include // for operator += - -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; - -typedef TypedSymbol Key; -typedef LieConfig VecConfig; -typedef NonlinearConstraint1 NLC1; -typedef NonlinearConstraint2 NLC2; - -/* ************************************************************************* */ -// unary functions with scalar variables -/* ************************************************************************* */ -namespace test1 { - - /** p = 1, g(x) = x^2-5 = 0 */ - Vector g(const VecConfig& config, const list& keys) { - double x = config[keys.front()](0); - return Vector_(1, x * x - 5); - } - - /** p = 1, jacobianG(x) = 2x */ - Matrix G(const VecConfig& config, const list& keys) { - double x = config[keys.front()](0); - return Matrix_(1, 1, 2 * x); - } - -} // \namespace test1 - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_scalar_construction ) { - // construct a constraint on x - size_t p = 1; - Key x1(1); - list keys; keys += x1; - NLC1 c1(boost::bind(test1::g, _1, keys), - x1, boost::bind(test1::G, _1, keys), - p, 10.0); - - // get a configuration to use for finding the error - VecConfig config; - config.insert(x1, Vector_(1, 1.0)); - - // calculate the error - Vector actualVec = c1.unwhitenedError(config); - Vector expectedVec = Vector_(1, -4.0); - CHECK(assert_equal(actualVec, expectedVec, 1e-5)); - - double actError = c1.error(config); - double expError = 160.0; - DOUBLES_EQUAL(expError, actError, 1e-5); -} - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_scalar_linearize ) { - size_t p = 1; - Key x1(1); - list keys; keys += x1; - NLC1 c1(boost::bind(test1::g, _1, keys), - x1, boost::bind(test1::G, _1, keys), - p); - - // get a configuration to use for linearization - VecConfig realconfig; - realconfig.insert(x1, Vector_(1, 1.0)); - - // linearize the system - GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig); - - // verify - SharedDiagonal model = noiseModel::Constrained::All(p); - Matrix Ax1 = Matrix_(p,1, 2.0); - Vector rhs = Vector_(p, 4.0); - GaussianFactor expectedFactor(x1, Ax1, rhs, model); - - CHECK(assert_equal(*linfactor, expectedFactor)); -} - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_scalar_equal ) { - Key x(0), y(1); - list keys1, keys2; keys1 += x; keys2 += y; - NLC1 - c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1), - c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1), - c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2), - c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1); - - CHECK(assert_equal(c1, c2)); - CHECK(assert_equal(c2, c1)); - CHECK(!c1.equals(c3)); - CHECK(!c1.equals(c4)); -} - -/* ************************************************************************* */ -// binary functions with scalar variables -/* ************************************************************************* */ -namespace test2 { - - /** p = 1, g(x) = x^2-5 -y = 0 */ - Vector g(const VecConfig& config, const list& keys) { - double x = config[keys.front()](0); - double y = config[keys.back()](0); - return Vector_(1, x * x - 5.0 - y); - } - - /** jacobian for x, jacobianG(x,y) in x: 2x*/ - Matrix G1(const VecConfig& config, const list& keys) { - double x = config[keys.front()](0); - return Matrix_(1, 1, 2.0 * x); - } - - /** jacobian for y, jacobianG(x,y) in y: -1 */ - Matrix G2(const VecConfig& config, const list& keys) { - return Matrix_(1, 1, -1.0); - } - -} // \namespace test2 - -/* ************************************************************************* */ -TEST( NonlinearConstraint2, binary_scalar_construction ) { - // construct a constraint on x and y - size_t p = 1; - Key x0(0), x1(1); - list keys; keys += x0, x1; - NLC2 c1( - boost::bind(test2::g, _1, keys), - x0, boost::bind(test2::G1, _1, keys), - x1, boost::bind(test2::G1, _1, keys), - p); - - // get a configuration to use for finding the error - VecConfig 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)); -} - -/* ************************************************************************* */ -TEST( NonlinearConstraint2, binary_scalar_linearize ) { - // create a constraint - size_t p = 1; - Key x0(0), x1(1); - list keys; keys += x0, x1; - NLC2 c1( - boost::bind(test2::g, _1, keys), - x0, boost::bind(test2::G1, _1, keys), - x1, boost::bind(test2::G2, _1, keys), - p); - - // get a configuration to use for finding the error - VecConfig realconfig; - realconfig.insert(x0, Vector_(1, 1.0)); - realconfig.insert(x1, Vector_(1, 2.0)); - - // linearize the system - GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig); - - // verify - Matrix Ax0 = Matrix_(1,1, 2.0), - Ax1 = Matrix_(1,1,-1.0); - Vector rhs = Vector_(1, 6.0); - SharedDiagonal expModel = noiseModel::Constrained::All(p); - GaussianFactor expFactor(x0,Ax0, x1, Ax1,rhs, expModel); - CHECK(assert_equal(expFactor, *actualFactor)); -} - -/* ************************************************************************* */ -TEST( NonlinearConstraint2, binary_scalar_equal ) { - list keys1, keys2, keys3; - Key x0(0), x1(1), x2(2); - keys1 += x0, x1; keys2 += x1, x0; keys3 += x0; - NLC2 - c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1), - c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1), - c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1), - c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3); - - CHECK(assert_equal(c1, c2)); - CHECK(assert_equal(c2, c1)); - CHECK(!c1.equals(c3)); - CHECK(!c1.equals(c4)); -} - -/* ************************************************************************* */ -// Inequality tests - DISABLED -/* ************************************************************************* */ -//namespace inequality1 { -// -// /** p = 1, g(x) x^2 - 5 > 0 */ -// Vector g(const VecConfig& 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 VecConfig& 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, false); // inequality constraint -// -// // get configurations to use for evaluation -// VecConfig 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, false); // inequality constraint -// -// // get configurations to use for linearization -// VecConfig config1, config2; -// config1.insert(x0, Vector_(1, 10.0)); // should have zero error -// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error -// -// // linearize for inactive constraint -// GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1); -// -// // check if the factor is active -// CHECK(!c1.active(config1)); -// -// // linearize for active constraint -// GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2); -// CHECK(c1.active(config2)); -// -// // verify -// Vector sigmas = Vector_(2, 1.0, 0.0); -// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas); -// GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0), -// L1, Matrix_(2,1, 1.0, 0.0), -// Vector_(2, 0.0, 4.0), model); -// -// CHECK(assert_equal(*actualFactor2, expectedFactor)); -//} - -/* ************************************************************************* */ -// Binding arbitrary functions -/* ************************************************************************* */ -//namespace binding1 { -// -// /** p = 1, g(x) x^2 - r > 0 */ -// Vector g(double r, const VecConfig& 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 VecConfig& 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, false); // inequality constraint -// -// // get configurations to use for evaluation -// VecConfig 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 VecConfig& 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 VecConfig& 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 VecConfig& config) { - return Matrix_(1, 1, -1.0 * c); - } - -} // \namespace binding2 - -/* ************************************************************************* */ -TEST( NonlinearConstraint2, binary_binding ) { - // construct a constraint on x and y - 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); - - // get a configuration to use for finding the error - VecConfig 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); } -/* ************************************************************************* */ diff --git a/slam/simulated2D.h b/slam/simulated2D.h index eae6b2ecf..b2bef5299 100644 --- a/slam/simulated2D.h +++ b/slam/simulated2D.h @@ -56,6 +56,7 @@ namespace gtsam { */ template struct GenericPrior: public NonlinearFactor1 { + typedef boost::shared_ptr > shared_ptr; Point2 z_; @@ -76,6 +77,7 @@ namespace gtsam { template struct GenericOdometry: public NonlinearFactor2 { + typedef boost::shared_ptr > shared_ptr; Point2 z_; GenericOdometry(const Point2& z, const SharedGaussian& model, @@ -97,7 +99,7 @@ namespace gtsam { class GenericMeasurement: public NonlinearFactor2 { public: - + typedef boost::shared_ptr > shared_ptr; Point2 z_; GenericMeasurement(const Point2& z, const SharedGaussian& model, diff --git a/slam/simulated2DConstraints.h b/slam/simulated2DConstraints.h new file mode 100644 index 000000000..77fddc713 --- /dev/null +++ b/slam/simulated2DConstraints.h @@ -0,0 +1,67 @@ +/** + * @file simulated2DConstraints.h + * @brief measurement functions and constraint definitions for simulated 2D robot + * @author Alex Cunningham + */ + +// \callgraph + +#pragma once + +#include +#include + +// \namespace + +namespace gtsam { + + namespace simulated2D { + + /** + * Unary factor encoding a hard equality on a Point + */ + template + struct GenericUnaryEqualityConstraint: public NonlinearConstraint1 { + typedef NonlinearConstraint1 Base; + typedef boost::shared_ptr > shared_ptr; + + Point2 z_; + + GenericUnaryEqualityConstraint(const Point2& z, const Key& key, double mu = 1000.0) : + Base(key, 2, mu), z_(z) { + } + + Vector evaluateError(const Point2& x, boost::optional H = + boost::none) const { + return (prior(x, H) - z_).vector(); + } + + }; + + /** + * Binary factor simulating "odometry" between two Vectors + */ + template + struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2 { + typedef NonlinearConstraint2 Base; + typedef boost::shared_ptr > shared_ptr; + Point2 z_; + + GenericOdoHardEqualityConstraint( + const Point2& z, const Key& i1, const Key& i2, double mu = 1000.0) : + Base (i1, i2, 2, mu), z_(z) { + } + + Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + return (odo(x1, x2, H1, H2) - z_).vector(); + } + + }; + + /** Typedefs for regular use */ + typedef GenericUnaryEqualityConstraint UnaryEqualityConstraint; + typedef GenericOdoHardEqualityConstraint OdoEqualityConstraint; + + } // namespace simulated2D +} // namespace gtsam diff --git a/tests/Makefile.am b/tests/Makefile.am index 72ba74bb4..6210bd1f1 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -8,8 +8,10 @@ check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGrap check_PROGRAMS += testGaussianISAM testGraph check_PROGRAMS += testInference testIterative testGaussianJunctionTree check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph -check_PROGRAMS += testNonlinearOptimizer testNonlinearConstraint testSubgraphPreconditioner +check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig +# check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged +check_PROGRAMS += testNonlinearEqualityConstraint if USE_LDL check_PROGRAMS += testConstraintOptimizer diff --git a/tests/testNonlinearConstraint.cpp b/tests/testNonlinearConstraint.cpp index 944bc4faa..232b2f3a4 100644 --- a/tests/testNonlinearConstraint.cpp +++ b/tests/testNonlinearConstraint.cpp @@ -1,6 +1,6 @@ /* - * @file testSQP.cpp - * @brief demos of SQP using existing gtsam components + * @file testNonlinearConstraint.cpp + * @brief demos of constrained optimization using existing gtsam components * @author Alex Cunningham */ @@ -10,1076 +10,856 @@ #include // for insert #include #include + #include -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include -#include -#include - -// templated implementations -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost; -using namespace boost::assign; - -// Models to use -SharedDiagonal probModel1 = sharedSigma(1,1.0); -SharedDiagonal probModel2 = sharedSigma(2,1.0); -SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); - -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - -/* ********************************************************************* */ -// full components -typedef simulated2D::Config Config2D; -typedef NonlinearFactorGraph Graph2D; -typedef NonlinearEquality NLE; -typedef boost::shared_ptr > shared; -typedef NonlinearOptimizer Optimizer; - -/* - * Determining a ground truth linear system - * with two poses seeing one landmark, with each pose - * constrained to a particular value - */ -TEST (NonlinearConstraint, two_pose_truth ) { - bool verbose = false; - - // create a graph - shared_ptr graph(new Graph2D); - - // add the constraints on the ends - // position (1, 1) constraint for x1 - // position (5, 6) constraint for x2 - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1); - Point2 pt_x1(1.0, 1.0), - pt_x2(5.0, 6.0); - shared_ptr ef1(new NLE(x1, pt_x1)), - ef2(new NLE(x2, pt_x2)); - graph->push_back(ef1); - graph->push_back(ef2); - - // measurement from x1 to l1 - Point2 z1(0.0, 5.0); - SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); - graph->push_back(f1); - - // measurement from x2 to l1 - Point2 z2(-4.0, 0.0); - shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l1)); - graph->push_back(f2); - - // create an initial estimate - Point2 pt_l1( - 1.0, 6.0 // ground truth - //1.2, 5.6 // small error - ); - shared_ptr initialEstimate(new Config2D); - initialEstimate->insert(l1, pt_l1); - initialEstimate->insert(x1, pt_x1); - initialEstimate->insert(x2, pt_x2); - - // optimize the graph - shared_ptr ordering(new Ordering()); - *ordering += "x1", "x2", "l1"; - Optimizer::shared_solver solver(new Optimizer::solver(ordering)); - Optimizer optimizer(graph, initialEstimate, solver); - - // display solution - double relativeThreshold = 1e-5; - double absoluteThreshold = 1e-5; - Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); - boost::shared_ptr actual = act_opt.config(); - if (verbose) actual->print("Configuration after optimization"); - - // verify - Config2D expected; - expected.insert(x1, pt_x1); - expected.insert(x2, pt_x2); - expected.insert(l1, Point2(1.0, 6.0)); - CHECK(assert_equal(expected, *actual, 1e-5)); -} - -/* ********************************************************************* */ -namespace sqp_test1 { - - // binary constraint between landmarks - /** g(x) = x-y = 0 */ - Vector g(const Config2D& config, const list& keys) { - Point2 pt1, pt2; - pt1 = config[simulated2D::PointKey(keys.front().index())]; - pt2 = config[simulated2D::PointKey(keys.back().index())]; - return Vector_(2, pt1.x() - pt2.x(), pt1.y() - pt2.y()); - } - - /** jacobian at l1 */ - Matrix G1(const Config2D& config, const list& keys) { - return eye(2); - } - - /** jacobian at l2 */ - Matrix G2(const Config2D& config, const list& keys) { - return -1 * eye(2); - } - -} // \namespace sqp_test1 - -namespace sqp_test2 { - - // Unary Constraint on x1 - /** g(x) = x -[1;1] = 0 */ - Vector g(const Config2D& config, const list& keys) { - Point2 x = config[keys.front()]; - return Vector_(2, x.x() - 1.0, x.y() - 1.0); - } - - /** jacobian at x1 */ - Matrix G(const Config2D& config, const list& keys) { - return eye(2); - } - -} // \namespace sqp_test2 - - -typedef NonlinearConstraint2< - Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; - -/* ********************************************************************* - * Version that actually uses nonlinear equality constraints - * to to perform optimization. Same as above, but no - * equality constraint on x2, and two landmarks that - * should be the same. Note that this is a linear system, - * so it will converge in one step. - */ -TEST (NonlinearConstraint, two_pose ) { - bool verbose = false; - - // create the graph - shared_ptr graph(new Graph2D); - - // add the constraints on the ends - // position (1, 1) constraint for x1 - // position (5, 6) constraint for x2 - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); - Point2 pt_x1(1.0, 1.0), - pt_x2(5.0, 6.0); - shared_ptr ef1(new NLE(x1, pt_x1)); - graph->push_back(ef1); - - // measurement from x1 to l1 - Point2 z1(0.0, 5.0); - SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); - graph->push_back(f1); - - // measurement from x2 to l2 - Point2 z2(-4.0, 0.0); - shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); - graph->push_back(f2); - - // equality constraint between l1 and l2 - list keys2; keys2 += l1, l2; - boost::shared_ptr c2(new NLC2( - boost::bind(sqp_test1::g, _1, keys2), - l1, boost::bind(sqp_test1::G1, _1, keys2), - l2, boost::bind(sqp_test1::G2, _1, keys2), - 2)); - graph->push_back(c2); - - if (verbose) graph->print("Initial nonlinear graph with constraints"); - - // create an initial estimate - shared_ptr initialEstimate(new Config2D); - initialEstimate->insert(x1, pt_x1); - initialEstimate->insert(x2, Point2()); - initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth - initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - - // create state config variables and initialize them - Config2D state(*initialEstimate); - - // linearize the graph - boost::shared_ptr fg = graph->linearize(state); - - if (verbose) fg->print("Linearized graph"); - - // create an ordering - Ordering ordering; - ordering += "x1", "x2", "l1", "l2"; - - // optimize linear graph to get full delta config - GaussianBayesNet cbn = fg->eliminate(ordering); - if (verbose) cbn.print("ChordalBayesNet"); - - VectorConfig delta = optimize(cbn); //fg.optimize(ordering); - if (verbose) delta.print("Delta Config"); - - // update both state variables - state = expmap(state, delta); - if (verbose) state.print("newState"); - - // verify - Config2D expected; - expected.insert(x1, pt_x1); - expected.insert(l1, Point2(1.0, 6.0)); - expected.insert(l2, Point2(1.0, 6.0)); - expected.insert(x2, Point2(5.0, 6.0)); - CHECK(assert_equal(expected, state, 1e-5)); -} - -/* ********************************************************************* */ -// VSLAM Examples -/* ********************************************************************* */ -// make a realistic calibration matrix -double fov = 60; // degrees -size_t w=640,h=480; -Cal3_S2 K(fov,w,h); -boost::shared_ptr shK(new Cal3_S2(K)); - -using namespace gtsam::visualSLAM; -using namespace boost; - -// typedefs for visual SLAM example -typedef TypedSymbol Pose3Key; -typedef TypedSymbol Point3Key; -//typedef TupleConfig3, -// LieConfig, -// LieConfig > VConfig; -typedef visualSLAM::Config VConfig; -typedef NonlinearFactorGraph VGraph; -typedef boost::shared_ptr > shared_vf; -typedef NonlinearOptimizer VOptimizer; -typedef NonlinearConstraint2< - VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2; -typedef NonlinearEquality Pose3Constraint; - -/** - * Ground truth for a visual SLAM example with stereo vision - */ -TEST (NonlinearConstraint, stereo_truth ) { - bool verbose = false; - - // create initial estimates - Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); - Pose3 pose1(faceDownY, Point3()); // origin, left camera - SimpleCamera camera1(K, pose1); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(K, pose2); - Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away - Point3 landmarkNoisy(1.0, 6.0, 0.0); - - // create truth config - boost::shared_ptr truthConfig(new VConfig); - truthConfig->insert(Pose3Key(1), camera1.pose()); - truthConfig->insert(Pose3Key(2), camera2.pose()); - truthConfig->insert(Point3Key(1), landmark); - - // create graph - shared_ptr graph(new VGraph()); - - // create equality constraints for poses - graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); - graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); - - // create VSLAM factors - Point2 z1 = camera1.project(landmark); - if (verbose) z1.print("z1"); - SharedDiagonal vmodel = noiseModel::Unit::Create(3); - //ProjectionFactor test_vf(z1, vmodel, Pose3Key(1), Point3Key(1), shK); - shared_vf vf1(new GenericProjectionFactor( - z1, vmodel, Pose3Key(1), Point3Key(1), shK)); - graph->push_back(vf1); - Point2 z2 = camera2.project(landmark); - if (verbose) z2.print("z2"); - shared_vf vf2(new GenericProjectionFactor( - z2, vmodel, Pose3Key(2), Point3Key(1), shK)); - graph->push_back(vf2); - - if (verbose) graph->print("Graph after construction"); - - // create ordering - shared_ptr ord(new Ordering()); - *ord += "x1", "x2", "l1"; - - // create optimizer - VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); - VOptimizer optimizer(graph, truthConfig, solver); - - // optimize - VOptimizer afterOneIteration = optimizer.iterate(); - - // verify - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - if (verbose) afterOneIteration.config()->print("After iteration"); - CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); -} - - -/* ********************************************************************* - * Ground truth for a visual SLAM example with stereo vision - * with some noise injected into the initial config - */ -TEST (NonlinearConstraint, stereo_truth_noisy ) { - bool verbose = false; - - // setting to determine how far away the noisy landmark is, - // given that the ground truth is 5m in front of the cameras - double noisyDist = 7.6; - - // create initial estimates - Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); - Pose3 pose1(faceDownY, Point3()); // origin, left camera - SimpleCamera camera1(K, pose1); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(K, pose2); - Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away - Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out - - // create truth config - boost::shared_ptr truthConfig(new VConfig); - truthConfig->insert(Pose3Key(1), camera1.pose()); - truthConfig->insert(Pose3Key(2), camera2.pose()); - truthConfig->insert(Point3Key(1), landmark); - - // create config - boost::shared_ptr noisyConfig(new VConfig); - noisyConfig->insert(Pose3Key(1), camera1.pose()); - noisyConfig->insert(Pose3Key(2), camera2.pose()); - noisyConfig->insert(Point3Key(1), landmarkNoisy); - - // create graph - shared_ptr graph(new VGraph()); - - // create equality constraints for poses - graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); - graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); - - // create VSLAM factors - Point2 z1 = camera1.project(landmark); - if (verbose) z1.print("z1"); - SharedDiagonal vmodel = noiseModel::Unit::Create(3); - shared_vf vf1(new GenericProjectionFactor( - z1, vmodel, Pose3Key(1), Point3Key(1), shK)); - graph->push_back(vf1); - Point2 z2 = camera2.project(landmark); - if (verbose) z2.print("z2"); - shared_vf vf2(new GenericProjectionFactor( - z2, vmodel, Pose3Key(2), Point3Key(1), shK)); - graph->push_back(vf2); - - if (verbose) { - graph->print("Graph after construction"); - noisyConfig->print("Initial config"); - } - - // create ordering - shared_ptr ord(new Ordering()); - *ord += "x1", "x2", "l1"; - - // create optimizer - VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); - VOptimizer optimizer0(graph, noisyConfig, solver); - - if (verbose) - cout << "Initial Error: " << optimizer0.error() << endl; - - // use Levenberg-Marquardt optimization - double relThresh = 1e-5, absThresh = 1e-5; - VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT)); - - // verify - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-5); - - // check if correct - if (verbose) { - optimizer.config()->print("After iteration"); - cout << "Final error: " << optimizer.error() << endl; - } - CHECK(assert_equal(*truthConfig,*(optimizer.config()), 1e-5)); -} - -/* ********************************************************************* */ -namespace sqp_stereo { - - // binary constraint between landmarks - /** g(x) = x-y = 0 */ - Vector g(const VConfig& config, const list& keys) { - return config[keys.front()].vector() - - config[keys.back()].vector(); - } - - /** jacobian at l1 */ - Matrix G1(const VConfig& config, const list& keys) { - return eye(3); - } - - /** jacobian at l2 */ - Matrix G2(const VConfig& config, const list& keys) { - return -1.0 * eye(3); - } - -} // \namespace sqp_stereo - -/* ********************************************************************* */ -boost::shared_ptr stereoExampleGraph() { - // create initial estimates - Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); - Pose3 pose1(faceDownY, Point3()); // origin, left camera - SimpleCamera camera1(K, pose1); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(K, pose2); - Point3 landmark1(1.0, 5.0, 0.0); //centered between the cameras, 5 units away - Point3 landmark2(1.0, 5.0, 0.0); - - // create graph - shared_ptr graph(new VGraph); - - // create equality constraints for poses - graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); - graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); - - // create factors - Point2 z1 = camera1.project(landmark1); - SharedDiagonal vmodel = noiseModel::Unit::Create(3); - shared_vf vf1(new GenericProjectionFactor( - z1, vmodel, Pose3Key(1), Point3Key(1), shK)); - graph->push_back(vf1); - Point2 z2 = camera2.project(landmark2); - shared_vf vf2(new GenericProjectionFactor( - z2, vmodel, Pose3Key(2), Point3Key(2), shK)); - graph->push_back(vf2); - - // create the binary equality constraint between the landmarks - // NOTE: this is really just a linear constraint that is exactly the same - // as the previous examples - visualSLAM::PointKey l1(1), l2(2); - list keys; keys += l1, l2; - shared_ptr c2( - new VNLC2(boost::bind(sqp_stereo::g, _1, keys), - l1, boost::bind(sqp_stereo::G1, _1, keys), - l2, boost::bind(sqp_stereo::G2, _1, keys), - 3)); - graph->push_back(c2); - - return graph; -} - -/* ********************************************************************* */ -boost::shared_ptr stereoExampleTruthConfig() { - // create initial estimates - Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); - Pose3 pose1(faceDownY, Point3()); // origin, left camera - SimpleCamera camera1(K, pose1); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(K, pose2); - Point3 landmark1(1.0, 5.0, 0.0); //centered between the cameras, 5 units away - Point3 landmark2(1.0, 5.0, 0.0); - - // create config - boost::shared_ptr truthConfig(new VConfig); - truthConfig->insert(Pose3Key(1), camera1.pose()); - truthConfig->insert(Pose3Key(2), camera2.pose()); - truthConfig->insert(Point3Key(1), landmark1); - truthConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place - //truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); - - return truthConfig; -} - -/* ********************************************************************* - * SQP version of the above stereo example, - * with the initial case as the ground truth - */ -TEST (NonlinearConstraint, stereo_sqp ) { - bool verbose = false; - - // get a graph - boost::shared_ptr graph = stereoExampleGraph(); - if (verbose) graph->print("Graph after construction"); - - // get the truth config - boost::shared_ptr truthConfig = stereoExampleTruthConfig(); - - // create ordering - shared_ptr ord(new Ordering()); - *ord += "x1", "x2", "l1", "l2"; - VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); - - // create optimizer - VOptimizer optimizer(graph, truthConfig, solver); - - // optimize - VOptimizer afterOneIteration = optimizer.iterate(); - - // check if correct - CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); -} - -/* ********************************************************************* - * SQP version of the above stereo example, - * with noise in the initial estimate - */ -TEST (NonlinearConstraint, stereo_sqp_noisy ) { - - // get a graph - boost::shared_ptr graph = stereoExampleGraph(); - - // create initial data - Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); - Pose3 pose1(faceDownY, Point3()); // origin, left camera - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away - Point3 landmark2(1.5, 5.0, 0.0); - - // noisy config - boost::shared_ptr initConfig(new VConfig); - initConfig->insert(Pose3Key(1), pose1); - initConfig->insert(Pose3Key(2), pose2); - initConfig->insert(Point3Key(1), landmark1); - initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place - - // create ordering - shared_ptr ord(new Ordering()); - *ord += "x1", "x2", "l1", "l2"; - VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); - - // create optimizer - VOptimizer optimizer(graph, initConfig, solver); - - // optimize - VOptimizer *pointer = new VOptimizer(optimizer); - for (int i=0;i<1;i++) { - VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); - delete pointer; - pointer = newOptimizer; - } - VOptimizer::shared_config actual = pointer->config(); - delete(pointer); - - // get the truth config - boost::shared_ptr truthConfig = stereoExampleTruthConfig(); - - // check if correct - CHECK(assert_equal(*truthConfig,*actual, 1e-5)); -} - -static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); - -namespace map_warp_example { -typedef NonlinearConstraint1< - Config2D, simulated2D::PoseKey, Point2> NLC1; -} // \namespace map_warp_example - -/* ********************************************************************* */ -// Example that moves two separate maps into the same frame of reference -// Note that this is a linear example, so it should converge in one step -/* ********************************************************************* */ - -namespace sqp_LinearMapWarp2 { -// binary constraint between landmarks -/** g(x) = x-y = 0 */ -Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) { - Point2 p = config[key1]-config[key2]; - return Vector_(2, p.x(), p.y()); -} - -/** jacobian at l1 */ -Matrix jac_g1(const Config2D& config) { - return eye(2); -} - -/** jacobian at l2 */ -Matrix jac_g2(const Config2D& config) { - return -1*eye(2); -} -} // \namespace sqp_LinearMapWarp2 - -namespace sqp_LinearMapWarp1 { -// Unary Constraint on x1 -/** g(x) = x -[1;1] = 0 */ -Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) { - Point2 p = config[key]-Point2(1.0, 1.0); - return Vector_(2, p.x(), p.y()); -} - -/** jacobian at x1 */ -Matrix jac_g(const Config2D& config) { - return eye(2); -} -} // \namespace sqp_LinearMapWarp12 - -//typedef NonlinearOptimizer Optimizer; - -/** - * Creates the graph with each robot seeing the landmark, and it is - * known that it is the same landmark - */ -boost::shared_ptr linearMapWarpGraph() { - using namespace map_warp_example; - // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); - - // constant constraint on x1 - shared_ptr c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), - x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), - 2)); - - // measurement from x1 to l1 - Point2 z1(0.0, 5.0); - shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); - - // measurement from x2 to l2 - Point2 z2(-4.0, 0.0); - shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); - - // equality constraint between l1 and l2 - shared_ptr c2 (new NLC2( - boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), - l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), - l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), - 2)); - - // construct the graph - boost::shared_ptr graph(new Graph2D()); - graph->push_back(c1); - graph->push_back(c2); - graph->push_back(f1); - graph->push_back(f2); - - return graph; -} - -/* ********************************************************************* */ -TEST ( SQPOptimizer, map_warp_initLam ) { - // get a graph - boost::shared_ptr graph = linearMapWarpGraph(); - - // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); - - // create an initial estimate - shared_ptr initialEstimate(new Config2D); - initialEstimate->insert(x1, Point2(1.0, 1.0)); - initialEstimate->insert(l1, Point2(1.0, 6.0)); - initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin - - // create an ordering - shared_ptr ordering(new Ordering()); - *ordering += "x1", "x2", "l1", "l2"; - - // create an optimizer - Optimizer::shared_solver solver(new Optimizer::solver(ordering)); - Optimizer optimizer(graph, initialEstimate, solver); - - // perform an iteration of optimization - Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); - - // get the config back out and verify - Config2D actual = *(oneIteration.config()); - Config2D expected; - expected.insert(x1, Point2(1.0, 1.0)); - expected.insert(l1, Point2(1.0, 6.0)); - expected.insert(l2, Point2(1.0, 6.0)); - expected.insert(x2, Point2(5.0, 6.0)); - CHECK(assert_equal(expected, actual)); -} - -/* ********************************************************************* */ -// This is an obstacle avoidance demo, where there is a trajectory of -// three points, where there is a circular obstacle in the middle. There -// is a binary inequality constraint connecting the obstacle to the -// states, which enforces a minimum distance. -/* ********************************************************************* */ - -//typedef NonlinearConstraint2 AvoidConstraint; -//typedef shared_ptr shared_a; -//typedef NonlinearEquality PoseNLConstraint; -//typedef shared_ptr shared_pc; -//typedef NonlinearEquality ObstacleConstraint; -//typedef shared_ptr shared_oc; +//#define GTSAM_MAGIC_KEY // +//#include +//#include +//#include +//#include +//#include +//#include +//#include // -//namespace sqp_avoid1 { -//// avoidance radius -//double radius = 1.0; +//// templated implementations +//#include +//#include +//#include +//#include // -//// binary avoidance constraint -///** g(x) = ||x2-obs||^2 - radius^2 > 0 */ -//Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) { -// double dist2 = config[x].dist(config[obs]); -// double thresh = radius*radius; -// return Vector_(1, dist2-thresh); -//} +//using namespace std; +//using namespace gtsam; +//using namespace boost; +//using namespace boost::assign; // -///** jacobian at pose */ -//Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) { -// Point2 p = config[x]-config[obs]; -// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y()); -//} +//// Models to use +//SharedDiagonal probModel1 = sharedSigma(1,1.0); +//SharedDiagonal probModel2 = sharedSigma(2,1.0); +//SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); // -///** jacobian at obstacle */ -//Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) { -// Point2 p = config[x]-config[obs]; -// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y()); -//} -//} -// -//pair obstacleAvoidGraph() { -// // Keys -// PoseKey x1(1), x2(2), x3(3); -// PointKey l1(1); -// LagrangeKey L20(20); -// -// // Constrained Points -// Point2 pt_x1, -// pt_x3(10.0, 0.0), -// pt_l1(5.0, -0.5); -// -// shared_pc e1(new PoseNLConstraint(x1, pt_x1)); -// shared_pc e2(new PoseNLConstraint(x3, pt_x3)); -// shared_oc e3(new ObstacleConstraint(l1, pt_l1)); -// -// // measurement from x1 to x2 -// Point2 x1x2(5.0, 0.0); -// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2)); -// -// // measurement from x2 to x3 -// Point2 x2x3(5.0, 0.0); -// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3)); -// -// // create a binary inequality constraint that forces the middle point away from -// // the obstacle -// shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1), -// x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1), -// l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1), -// 1, L20, 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); -// -// // make a config of the fixed values, for convenience -// Config2D config; -// config.insert(x1, pt_x1); -// config.insert(x3, pt_x3); -// config.insert(l1, pt_l1); -// -// return make_pair(graph, config); -//} +//// trick from some reading group +//#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) // ///* ********************************************************************* */ -//TEST ( SQPOptimizer, inequality_avoid ) { -// // create the graph -// NLGraph graph; Config2D feasible; -// boost::tie(graph, feasible) = obstacleAvoidGraph(); +//// full components +//typedef simulated2D::Config Config2D; +//typedef NonlinearFactorGraph Graph2D; +//typedef NonlinearEquality NLE; +//typedef boost::shared_ptr > shared; +//typedef NonlinearOptimizer Optimizer; // -// // create the rest of the config -// shared_ptr init(new Config2D(feasible)); -// PoseKey x2(2); -// init->insert(x2, Point2(5.0, 100.0)); +///* +// * Determining a ground truth linear system +// * with two poses seeing one landmark, with each pose +// * constrained to a particular value +// */ +//TEST (NonlinearConstraint, two_pose_truth ) { +// // create a graph +// shared_ptr graph(new Graph2D); // -// // create an ordering -// Ordering ord; -// ord += "x1", "x2", "x3", "l1"; +// // add the constraints on the ends +// // position (1, 1) constraint for x1 +// // position (5, 6) constraint for x2 +// simulated2D::PoseKey x1(1), x2(2); +// simulated2D::PointKey l1(1); +// Point2 pt_x1(1.0, 1.0), +// pt_x2(5.0, 6.0); +// shared_ptr ef1(new NLE(x1, pt_x1)), +// ef2(new NLE(x2, pt_x2)); +// graph->push_back(ef1); +// graph->push_back(ef2); // -// // create an optimizer -// Optimizer optimizer(graph, ord, init); +// // measurement from x1 to l1 +// Point2 z1(0.0, 5.0); +// SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); +// shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); +// graph->push_back(f1); // -// // perform an iteration of optimization -// // NOTE: the constraint will be inactive in the first iteration, -// // so it will violate the constraint after one iteration -// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); +// // measurement from x2 to l1 +// Point2 z2(-4.0, 0.0); +// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l1)); +// graph->push_back(f2); // -// Config2D exp1(feasible); -// exp1.insert(x2, Point2(5.0, 0.0)); -// CHECK(assert_equal(exp1, *(afterOneIteration.config()))); +// // create an initial estimate +// Point2 pt_l1( +// 1.0, 6.0 // ground truth +// //1.2, 5.6 // small error +// ); +// shared_ptr initialEstimate(new Config2D); +// initialEstimate->insert(l1, pt_l1); +// initialEstimate->insert(x1, pt_x1); +// initialEstimate->insert(x2, pt_x2); // -// // the second iteration will activate the constraint and force the -// // config to a viable configuration. -// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT); -// -// Config2D exp2(feasible); -// exp2.insert(x2, Point2(5.0, 0.5)); -// CHECK(assert_equal(exp2, *(after2ndIteration.config()))); -//} - -///* ********************************************************************* */ -//TEST ( SQPOptimizer, inequality_avoid_iterative ) { -// // create the graph -// NLGraph graph; Config2D feasible; -// boost::tie(graph, feasible) = obstacleAvoidGraph(); -// -// // create the rest of the config -// shared_ptr init(new Config2D(feasible)); -// PoseKey x2(2); -// init->insert(x2, Point2(5.0, 100.0)); -// -// // create an ordering -// Ordering ord; -// ord += "x1", "x2", "x3", "l1"; -// -// // 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); +// // optimize the graph +// boost::shared_ptr actual = Optimizer::optimizeGN(graph, initialEstimate); // // // verify -// Config2D exp2(feasible); -// exp2.insert(x2, Point2(5.0, 0.5)); -// CHECK(assert_equal(exp2, *(final.config()))); +// Config2D expected; +// expected.insert(x1, pt_x1); +// expected.insert(x2, pt_x2); +// expected.insert(l1, Point2(1.0, 6.0)); +// CHECK(assert_equal(expected, *actual, 1e-5)); //} - -/* ********************************************************************* - * Example from SQP testing: - * - * This example uses a nonlinear objective function and - * nonlinear equality constraint. The formulation is actually - * the Cholesky form that creates the full Hessian explicitly, - * which should really be avoided with our QR-based machinery. - * - * Note: the update equation used here has a fixed step size - * and gain that is rather arbitrarily chosen, and as such, - * will take a silly number of iterations. - */ -TEST (NonlinearConstraint, problem1_cholesky ) { - bool verbose = false; - // use a nonlinear function of f(x) = x^2+y^2 - // nonlinear equality constraint: g(x) = x^2-5-y=0 - // Lagrangian: f(x) + \lambda*g(x) - - // Symbols - Symbol x1("x1"), y1("y1"), L1("L1"); - - // state structure: [x y \lambda] - VectorConfig init, state; - init.insert(x1, Vector_(1, 1.0)); - init.insert(y1, Vector_(1, 1.0)); - init.insert(L1, Vector_(1, 1.0)); - state = init; - - if (verbose) init.print("Initial State"); - - // loop until convergence - int maxIt = 10; - for (int i = 0; i ||Ax-b||^2 - * where: - * h(x) simply returns the inputs - * z zeros(2) - * A identity - * b linearization point - */ - Matrix A = eye(2); - Vector b = Vector_(2, x, y); - GaussianFactor::shared_ptr f1( - new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1) - y1, sub(A, 0,2, 1,2), // A(:,2) - b, // rhs of f(x) - probModel2)); // arbitrary sigma - - /** create the constraint-linear factor - * Provides a mechanism to use variable gain to force the constraint - * \lambda*gradG*dx + d\lambda = zero - * formulated in matrix form as: - * [\lambda*gradG eye(1)] [dx; d\lambda] = zero - */ - Matrix gradG = Matrix_(1, 2,2*x, -1.0); - GaussianFactor::shared_ptr f2( - new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) - y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) - L1, eye(1), // dlambda term - Vector_(1, 0.0), // rhs is zero - probModel1)); // arbitrary sigma - - // create the actual constraint - // [gradG] [x; y] - g = 0 - Vector g = Vector_(1,x*x-y-5); - GaussianFactor::shared_ptr c1( - new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG - y1, sub(gradG, 0,1, 1,2), // slice second part of gradG - g, // value of constraint function - constraintModel1)); // force to constraint - - // construct graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - fg.push_back(c1); - if (verbose) fg.print("Graph"); - - // solve - Ordering ord; - ord += x1, y1, L1; - VectorConfig delta = fg.optimize(ord); - if (verbose) delta.print("Delta"); - - // update initial estimate - VectorConfig newState = expmap(state, delta.scale(-1.0)); - - // set the state to the updated state - state = newState; - - if (verbose) state.print("Updated State"); - } - - // verify that it converges to the nearest optimal point - VectorConfig expected; - expected.insert(x1, Vector_(1, 2.12)); - expected.insert(y1, Vector_(1, -0.5)); - CHECK(assert_equal(state[x1], expected[x1], 1e-2)); - CHECK(assert_equal(state[y1], expected[y1], 1e-2)); -} +// +///* ********************************************************************* */ +//namespace constrained_test1 { +// +// // binary constraint between landmarks +// /** g(x) = x-y = 0 */ +// Vector g(const Config2D& config, const list& keys) { +// Point2 pt1, pt2; +// pt1 = config[simulated2D::PointKey(keys.front().index())]; +// pt2 = config[simulated2D::PointKey(keys.back().index())]; +// return Vector_(2, pt1.x() - pt2.x(), pt1.y() - pt2.y()); +// } +// +// /** jacobian at l1 */ +// Matrix G1(const Config2D& config, const list& keys) { +// return eye(2); +// } +// +// /** jacobian at l2 */ +// Matrix G2(const Config2D& config, const list& keys) { +// return -1 * eye(2); +// } +// +//} // \namespace constrained_test1 +// +//namespace constrained_test2 { +// +// // Unary Constraint on x1 +// /** g(x) = x -[1;1] = 0 */ +// Vector g(const Config2D& config, const list& keys) { +// Point2 x = config[keys.front()]; +// return Vector_(2, x.x() - 1.0, x.y() - 1.0); +// } +// +// /** jacobian at x1 */ +// Matrix G(const Config2D& config, const list& keys) { +// return eye(2); +// } +// +//} // \namespace constrained_test2 +// +// +//typedef NonlinearConstraint2< +// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; +// +///* ********************************************************************* +// * Version that actually uses nonlinear equality constraints +// * to to perform optimization. Same as above, but no +// * equality constraint on x2, and two landmarks that +// * should be the same. Note that this is a linear system, +// * so it will converge in one step. +// */ +//TEST (NonlinearConstraint, two_pose ) { +// bool verbose = false; +// +// // create the graph +// shared_ptr graph(new Graph2D); +// +// // add the constraints on the ends +// // position (1, 1) constraint for x1 +// // position (5, 6) constraint for x2 +// simulated2D::PoseKey x1(1), x2(2); +// simulated2D::PointKey l1(1), l2(2); +// Point2 pt_x1(1.0, 1.0), +// pt_x2(5.0, 6.0); +// shared_ptr ef1(new NLE(x1, pt_x1)); +// graph->push_back(ef1); +// +// // measurement from x1 to l1 +// Point2 z1(0.0, 5.0); +// SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); +// shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); +// graph->push_back(f1); +// +// // measurement from x2 to l2 +// Point2 z2(-4.0, 0.0); +// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); +// graph->push_back(f2); +// +// // equality constraint between l1 and l2 +// list keys2; keys2 += l1, l2; +// boost::shared_ptr c2(new NLC2( +// boost::bind(constrained_test1::g, _1, keys2), +// l1, boost::bind(constrained_test1::G1, _1, keys2), +// l2, boost::bind(constrained_test1::G2, _1, keys2), +// 2)); +// graph->push_back(c2); +// +// if (verbose) graph->print("Initial nonlinear graph with constraints"); +// +// // create an initial estimate +// shared_ptr initialEstimate(new Config2D); +// initialEstimate->insert(x1, pt_x1); +// initialEstimate->insert(x2, Point2()); +// initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth +// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame +// +// // create state config variables and initialize them +// Config2D state(*initialEstimate); +// +// // linearize the graph +// boost::shared_ptr fg = graph->linearize(state); +// +// if (verbose) fg->print("Linearized graph"); +// +// // create an ordering +// Ordering ordering; +// ordering += "x1", "x2", "l1", "l2"; +// +// // optimize linear graph to get full delta config +// GaussianBayesNet cbn = fg->eliminate(ordering); +// if (verbose) cbn.print("ChordalBayesNet"); +// +// VectorConfig delta = optimize(cbn); //fg.optimize(ordering); +// if (verbose) delta.print("Delta Config"); +// +// // update both state variables +// state = expmap(state, delta); +// if (verbose) state.print("newState"); +// +// // verify +// Config2D expected; +// expected.insert(x1, pt_x1); +// expected.insert(l1, Point2(1.0, 6.0)); +// expected.insert(l2, Point2(1.0, 6.0)); +// expected.insert(x2, Point2(5.0, 6.0)); +// CHECK(assert_equal(expected, state, 1e-5)); +//} +// +///* ********************************************************************* */ +//// VSLAM Examples +///* ********************************************************************* */ +//// make a realistic calibration matrix +//double fov = 60; // degrees +//size_t w=640,h=480; +//Cal3_S2 K(fov,w,h); +//boost::shared_ptr shK(new Cal3_S2(K)); +// +//using namespace gtsam::visualSLAM; +//using namespace boost; +// +//// typedefs for visual SLAM example +//typedef TypedSymbol Pose3Key; +//typedef TypedSymbol Point3Key; +////typedef TupleConfig3, +//// LieConfig, +//// LieConfig > VConfig; +//typedef visualSLAM::Config VConfig; +//typedef NonlinearFactorGraph VGraph; +//typedef boost::shared_ptr > shared_vf; +//typedef NonlinearOptimizer VOptimizer; +//typedef NonlinearConstraint2< +// VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2; +//typedef NonlinearEquality Pose3Constraint; +// +///** +// * Ground truth for a visual SLAM example with stereo vision +// */ +//TEST (NonlinearConstraint, stereo_truth ) { +// bool verbose = false; +// +// // create initial estimates +// Rot3 faceDownY(Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 0.0, 1.0, +// 0.0, 1.0, 0.0)); +// Pose3 pose1(faceDownY, Point3()); // origin, left camera +// SimpleCamera camera1(K, pose1); +// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left +// SimpleCamera camera2(K, pose2); +// Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away +// Point3 landmarkNoisy(1.0, 6.0, 0.0); +// +// // create truth config +// boost::shared_ptr truthConfig(new VConfig); +// truthConfig->insert(Pose3Key(1), camera1.pose()); +// truthConfig->insert(Pose3Key(2), camera2.pose()); +// truthConfig->insert(Point3Key(1), landmark); +// +// // create graph +// shared_ptr graph(new VGraph()); +// +// // create equality constraints for poses +// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); +// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); +// +// // create VSLAM factors +// Point2 z1 = camera1.project(landmark); +// if (verbose) z1.print("z1"); +// SharedDiagonal vmodel = noiseModel::Unit::Create(3); +// //ProjectionFactor test_vf(z1, vmodel, Pose3Key(1), Point3Key(1), shK); +// shared_vf vf1(new GenericProjectionFactor( +// z1, vmodel, Pose3Key(1), Point3Key(1), shK)); +// graph->push_back(vf1); +// Point2 z2 = camera2.project(landmark); +// if (verbose) z2.print("z2"); +// shared_vf vf2(new GenericProjectionFactor( +// z2, vmodel, Pose3Key(2), Point3Key(1), shK)); +// graph->push_back(vf2); +// +// if (verbose) graph->print("Graph after construction"); +// +// // create ordering +// shared_ptr ord(new Ordering()); +// *ord += "x1", "x2", "l1"; +// +// // create optimizer +// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); +// VOptimizer optimizer(graph, truthConfig, solver); +// +// // optimize +// VOptimizer afterOneIteration = optimizer.iterate(); +// +// // verify +// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); +// +// // check if correct +// if (verbose) afterOneIteration.config()->print("After iteration"); +// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); +//} +// +// +///* ********************************************************************* +// * Ground truth for a visual SLAM example with stereo vision +// * with some noise injected into the initial config +// */ +//TEST (NonlinearConstraint, stereo_truth_noisy ) { +// bool verbose = false; +// +// // setting to determine how far away the noisy landmark is, +// // given that the ground truth is 5m in front of the cameras +// double noisyDist = 7.6; +// +// // create initial estimates +// Rot3 faceDownY(Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 0.0, 1.0, +// 0.0, 1.0, 0.0)); +// Pose3 pose1(faceDownY, Point3()); // origin, left camera +// SimpleCamera camera1(K, pose1); +// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left +// SimpleCamera camera2(K, pose2); +// Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away +// Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out +// +// // create truth config +// boost::shared_ptr truthConfig(new VConfig); +// truthConfig->insert(Pose3Key(1), camera1.pose()); +// truthConfig->insert(Pose3Key(2), camera2.pose()); +// truthConfig->insert(Point3Key(1), landmark); +// +// // create config +// boost::shared_ptr noisyConfig(new VConfig); +// noisyConfig->insert(Pose3Key(1), camera1.pose()); +// noisyConfig->insert(Pose3Key(2), camera2.pose()); +// noisyConfig->insert(Point3Key(1), landmarkNoisy); +// +// // create graph +// shared_ptr graph(new VGraph()); +// +// // create equality constraints for poses +// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); +// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); +// +// // create VSLAM factors +// Point2 z1 = camera1.project(landmark); +// if (verbose) z1.print("z1"); +// SharedDiagonal vmodel = noiseModel::Unit::Create(3); +// shared_vf vf1(new GenericProjectionFactor( +// z1, vmodel, Pose3Key(1), Point3Key(1), shK)); +// graph->push_back(vf1); +// Point2 z2 = camera2.project(landmark); +// if (verbose) z2.print("z2"); +// shared_vf vf2(new GenericProjectionFactor( +// z2, vmodel, Pose3Key(2), Point3Key(1), shK)); +// graph->push_back(vf2); +// +// if (verbose) { +// graph->print("Graph after construction"); +// noisyConfig->print("Initial config"); +// } +// +// // create ordering +// shared_ptr ord(new Ordering()); +// *ord += "x1", "x2", "l1"; +// +// // create optimizer +// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); +// VOptimizer optimizer0(graph, noisyConfig, solver); +// +// if (verbose) +// cout << "Initial Error: " << optimizer0.error() << endl; +// +// // use Levenberg-Marquardt optimization +// double relThresh = 1e-5, absThresh = 1e-5; +// VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT)); +// +// // verify +// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-5); +// +// // check if correct +// if (verbose) { +// optimizer.config()->print("After iteration"); +// cout << "Final error: " << optimizer.error() << endl; +// } +// CHECK(assert_equal(*truthConfig,*(optimizer.config()), 1e-5)); +//} +// +///* ********************************************************************* */ +//namespace constrained_stereo { +// +// // binary constraint between landmarks +// /** g(x) = x-y = 0 */ +// Vector g(const VConfig& config, const list& keys) { +// return config[keys.front()].vector() +// - config[keys.back()].vector(); +// } +// +// /** jacobian at l1 */ +// Matrix G1(const VConfig& config, const list& keys) { +// return eye(3); +// } +// +// /** jacobian at l2 */ +// Matrix G2(const VConfig& config, const list& keys) { +// return -1.0 * eye(3); +// } +// +//} // \namespace constrained_stereo +// +///* ********************************************************************* */ +//boost::shared_ptr stereoExampleGraph() { +// // create initial estimates +// Rot3 faceDownY(Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 0.0, 1.0, +// 0.0, 1.0, 0.0)); +// Pose3 pose1(faceDownY, Point3()); // origin, left camera +// SimpleCamera camera1(K, pose1); +// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left +// SimpleCamera camera2(K, pose2); +// Point3 landmark1(1.0, 5.0, 0.0); //centered between the cameras, 5 units away +// Point3 landmark2(1.0, 5.0, 0.0); +// +// // create graph +// shared_ptr graph(new VGraph); +// +// // create equality constraints for poses +// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(1), camera1.pose()))); +// graph->push_back(shared_ptr(new Pose3Constraint(Pose3Key(2), camera2.pose()))); +// +// // create factors +// Point2 z1 = camera1.project(landmark1); +// SharedDiagonal vmodel = noiseModel::Unit::Create(3); +// shared_vf vf1(new GenericProjectionFactor( +// z1, vmodel, Pose3Key(1), Point3Key(1), shK)); +// graph->push_back(vf1); +// Point2 z2 = camera2.project(landmark2); +// shared_vf vf2(new GenericProjectionFactor( +// z2, vmodel, Pose3Key(2), Point3Key(2), shK)); +// graph->push_back(vf2); +// +// // create the binary equality constraint between the landmarks +// // NOTE: this is really just a linear constraint that is exactly the same +// // as the previous examples +// visualSLAM::PointKey l1(1), l2(2); +// list keys; keys += l1, l2; +// shared_ptr c2( +// new VNLC2(boost::bind(constrained_stereo::g, _1, keys), +// l1, boost::bind(constrained_stereo::G1, _1, keys), +// l2, boost::bind(constrained_stereo::G2, _1, keys), +// 3)); +// graph->push_back(c2); +// +// return graph; +//} +// +///* ********************************************************************* */ +//boost::shared_ptr stereoExampleTruthConfig() { +// // create initial estimates +// Rot3 faceDownY(Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 0.0, 1.0, +// 0.0, 1.0, 0.0)); +// Pose3 pose1(faceDownY, Point3()); // origin, left camera +// SimpleCamera camera1(K, pose1); +// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left +// SimpleCamera camera2(K, pose2); +// Point3 landmark1(1.0, 5.0, 0.0); //centered between the cameras, 5 units away +// Point3 landmark2(1.0, 5.0, 0.0); +// +// // create config +// boost::shared_ptr truthConfig(new VConfig); +// truthConfig->insert(Pose3Key(1), camera1.pose()); +// truthConfig->insert(Pose3Key(2), camera2.pose()); +// truthConfig->insert(Point3Key(1), landmark1); +// truthConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place +// //truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); +// +// return truthConfig; +//} +// +///* ********************************************************************* +// * SQP version of the above stereo example, +// * with the initial case as the ground truth +// */ +//TEST (NonlinearConstraint, stereo_constrained ) { +// bool verbose = false; +// +// // get a graph +// boost::shared_ptr graph = stereoExampleGraph(); +// if (verbose) graph->print("Graph after construction"); +// +// // get the truth config +// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); +// +// // create ordering +// shared_ptr ord(new Ordering()); +// *ord += "x1", "x2", "l1", "l2"; +// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); +// +// // create optimizer +// VOptimizer optimizer(graph, truthConfig, solver); +// +// // optimize +// VOptimizer afterOneIteration = optimizer.iterate(); +// +// // check if correct +// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); +//} +// +///* ********************************************************************* +// * SQP version of the above stereo example, +// * with noise in the initial estimate +// */ +//TEST (NonlinearConstraint, stereo_constrained_noisy ) { +// +// // get a graph +// boost::shared_ptr graph = stereoExampleGraph(); +// +// // create initial data +// Rot3 faceDownY(Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 0.0, 1.0, +// 0.0, 1.0, 0.0)); +// Pose3 pose1(faceDownY, Point3()); // origin, left camera +// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left +// Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away +// Point3 landmark2(1.5, 5.0, 0.0); +// +// // noisy config +// boost::shared_ptr initConfig(new VConfig); +// initConfig->insert(Pose3Key(1), pose1); +// initConfig->insert(Pose3Key(2), pose2); +// initConfig->insert(Point3Key(1), landmark1); +// initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place +// +// // create ordering +// shared_ptr ord(new Ordering()); +// *ord += "x1", "x2", "l1", "l2"; +// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); +// +// // create optimizer +// VOptimizer optimizer(graph, initConfig, solver); +// +// // optimize +// VOptimizer *pointer = new VOptimizer(optimizer); +// for (int i=0;i<1;i++) { +// VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); +// delete pointer; +// pointer = newOptimizer; +// } +// VOptimizer::shared_config actual = pointer->config(); +// delete(pointer); +// +// // get the truth config +// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); +// +// // check if correct +// CHECK(assert_equal(*truthConfig,*actual, 1e-5)); +//} +// +//static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); +// +//namespace map_warp_example { +//typedef NonlinearConstraint1< +// Config2D, simulated2D::PoseKey, Point2> NLC1; +//} // \namespace map_warp_example +// +///* ********************************************************************* */ +//// Example that moves two separate maps into the same frame of reference +//// Note that this is a linear example, so it should converge in one step +///* ********************************************************************* */ +// +//namespace constrained_LinearMapWarp2 { +//// binary constraint between landmarks +///** g(x) = x-y = 0 */ +//Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) { +// Point2 p = config[key1]-config[key2]; +// return Vector_(2, p.x(), p.y()); +//} +// +///** jacobian at l1 */ +//Matrix jac_g1(const Config2D& config) { +// return eye(2); +//} +// +///** jacobian at l2 */ +//Matrix jac_g2(const Config2D& config) { +// return -1*eye(2); +//} +//} // \namespace constrained_LinearMapWarp2 +// +//namespace constrained_LinearMapWarp1 { +//// Unary Constraint on x1 +///** g(x) = x -[1;1] = 0 */ +//Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) { +// Point2 p = config[key]-Point2(1.0, 1.0); +// return Vector_(2, p.x(), p.y()); +//} +// +///** jacobian at x1 */ +//Matrix jac_g(const Config2D& config) { +// return eye(2); +//} +//} // \namespace constrained_LinearMapWarp12 +// +////typedef NonlinearOptimizer Optimizer; +// +///** +// * Creates the graph with each robot seeing the landmark, and it is +// * known that it is the same landmark +// */ +//boost::shared_ptr linearMapWarpGraph() { +// using namespace map_warp_example; +// // keys +// simulated2D::PoseKey x1(1), x2(2); +// simulated2D::PointKey l1(1), l2(2); +// +// // constant constraint on x1 +// shared_ptr c1(new NLC1(boost::bind(constrained_LinearMapWarp1::g_func, _1, x1), +// x1, boost::bind(constrained_LinearMapWarp1::jac_g, _1), +// 2)); +// +// // measurement from x1 to l1 +// Point2 z1(0.0, 5.0); +// shared f1(new simulated2D::GenericMeasurement(z1, sigma, x1,l1)); +// +// // measurement from x2 to l2 +// Point2 z2(-4.0, 0.0); +// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); +// +// // equality constraint between l1 and l2 +// shared_ptr c2 (new NLC2( +// boost::bind(constrained_LinearMapWarp2::g_func, _1, l1, l2), +// l1, boost::bind(constrained_LinearMapWarp2::jac_g1, _1), +// l2, boost::bind(constrained_LinearMapWarp2::jac_g2, _1), +// 2)); +// +// // construct the graph +// boost::shared_ptr graph(new Graph2D()); +// graph->push_back(c1); +// graph->push_back(c2); +// graph->push_back(f1); +// graph->push_back(f2); +// +// return graph; +//} +// +///* ********************************************************************* */ +//TEST ( SQPOptimizer, map_warp_initLam ) { +// // get a graph +// boost::shared_ptr graph = linearMapWarpGraph(); +// +// // keys +// simulated2D::PoseKey x1(1), x2(2); +// simulated2D::PointKey l1(1), l2(2); +// +// // create an initial estimate +// shared_ptr initialEstimate(new Config2D); +// initialEstimate->insert(x1, Point2(1.0, 1.0)); +// initialEstimate->insert(l1, Point2(1.0, 6.0)); +// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame +// initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin +// +// // create an ordering +// shared_ptr ordering(new Ordering()); +// *ordering += "x1", "x2", "l1", "l2"; +// +// // create an optimizer +// Optimizer::shared_solver solver(new Optimizer::solver(ordering)); +// Optimizer optimizer(graph, initialEstimate, solver); +// +// // perform an iteration of optimization +// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); +// +// // get the config back out and verify +// Config2D actual = *(oneIteration.config()); +// Config2D expected; +// expected.insert(x1, Point2(1.0, 1.0)); +// expected.insert(l1, Point2(1.0, 6.0)); +// expected.insert(l2, Point2(1.0, 6.0)); +// expected.insert(x2, Point2(5.0, 6.0)); +// CHECK(assert_equal(expected, actual)); +//} +// +///* ********************************************************************* */ +//// This is an obstacle avoidance demo, where there is a trajectory of +//// three points, where there is a circular obstacle in the middle. There +//// is a binary inequality constraint connecting the obstacle to the +//// states, which enforces a minimum distance. +///* ********************************************************************* */ +// +////typedef NonlinearConstraint2 AvoidConstraint; +////typedef shared_ptr shared_a; +////typedef NonlinearEquality PoseNLConstraint; +////typedef shared_ptr shared_pc; +////typedef NonlinearEquality ObstacleConstraint; +////typedef shared_ptr shared_oc; +//// +//// +////namespace constrained_avoid1 { +////// avoidance radius +////double radius = 1.0; +//// +////// binary avoidance constraint +/////** g(x) = ||x2-obs||^2 - radius^2 > 0 */ +////Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) { +//// double dist2 = config[x].dist(config[obs]); +//// double thresh = radius*radius; +//// return Vector_(1, dist2-thresh); +////} +//// +/////** jacobian at pose */ +////Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) { +//// Point2 p = config[x]-config[obs]; +//// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y()); +////} +//// +/////** jacobian at obstacle */ +////Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) { +//// Point2 p = config[x]-config[obs]; +//// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y()); +////} +////} +//// +////pair obstacleAvoidGraph() { +//// // Keys +//// PoseKey x1(1), x2(2), x3(3); +//// PointKey l1(1); +//// LagrangeKey L20(20); +//// +//// // Constrained Points +//// Point2 pt_x1, +//// pt_x3(10.0, 0.0), +//// pt_l1(5.0, -0.5); +//// +//// shared_pc e1(new PoseNLConstraint(x1, pt_x1)); +//// shared_pc e2(new PoseNLConstraint(x3, pt_x3)); +//// shared_oc e3(new ObstacleConstraint(l1, pt_l1)); +//// +//// // measurement from x1 to x2 +//// Point2 x1x2(5.0, 0.0); +//// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2)); +//// +//// // measurement from x2 to x3 +//// Point2 x2x3(5.0, 0.0); +//// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3)); +//// +//// // create a binary inequality constraint that forces the middle point away from +//// // the obstacle +//// shared_a c1(new AvoidConstraint(boost::bind(constrained_avoid1::g_func, _1, x2, l1), +//// x2, boost::bind(constrained_avoid1::jac_g1, _1, x2, l1), +//// l1,boost::bind(constrained_avoid1::jac_g2, _1, x2, l1), +//// 1, L20, 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); +//// +//// // make a config of the fixed values, for convenience +//// Config2D config; +//// config.insert(x1, pt_x1); +//// config.insert(x3, pt_x3); +//// config.insert(l1, pt_l1); +//// +//// return make_pair(graph, config); +////} +//// +/////* ********************************************************************* */ +////TEST ( SQPOptimizer, inequality_avoid ) { +//// // create the graph +//// NLGraph graph; Config2D feasible; +//// boost::tie(graph, feasible) = obstacleAvoidGraph(); +//// +//// // create the rest of the config +//// shared_ptr init(new Config2D(feasible)); +//// PoseKey x2(2); +//// init->insert(x2, Point2(5.0, 100.0)); +//// +//// // create an ordering +//// Ordering ord; +//// ord += "x1", "x2", "x3", "l1"; +//// +//// // create an optimizer +//// Optimizer optimizer(graph, ord, init); +//// +//// // perform an iteration of optimization +//// // NOTE: the constraint will be inactive in the first iteration, +//// // so it will violate the constraint after one iteration +//// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); +//// +//// Config2D exp1(feasible); +//// exp1.insert(x2, Point2(5.0, 0.0)); +//// CHECK(assert_equal(exp1, *(afterOneIteration.config()))); +//// +//// // the second iteration will activate the constraint and force the +//// // config to a viable configuration. +//// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT); +//// +//// Config2D exp2(feasible); +//// exp2.insert(x2, Point2(5.0, 0.5)); +//// CHECK(assert_equal(exp2, *(after2ndIteration.config()))); +////} +// +/////* ********************************************************************* */ +////TEST ( SQPOptimizer, inequality_avoid_iterative ) { +//// // create the graph +//// NLGraph graph; Config2D feasible; +//// boost::tie(graph, feasible) = obstacleAvoidGraph(); +//// +//// // create the rest of the config +//// shared_ptr init(new Config2D(feasible)); +//// PoseKey x2(2); +//// init->insert(x2, Point2(5.0, 100.0)); +//// +//// // create an ordering +//// Ordering ord; +//// ord += "x1", "x2", "x3", "l1"; +//// +//// // 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 +//// Config2D exp2(feasible); +//// exp2.insert(x2, Point2(5.0, 0.5)); +//// CHECK(assert_equal(exp2, *(final.config()))); +////} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp new file mode 100644 index 000000000..8937315d7 --- /dev/null +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -0,0 +1,191 @@ +/** + * @file testNonlinearEqualityConstraint.cpp + * @author Alex Cunningham + */ + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const double tol = 1e-9; + +SharedDiagonal hard_model = noiseModel::Constrained::All(2); +SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); + +typedef NonlinearFactorGraph Graph; +typedef boost::shared_ptr shared_graph; +typedef boost::shared_ptr shared_config; +typedef NonlinearOptimizer Optimizer; + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, unary_basics ) { + Point2 pt(1.0, 2.0); + simulated2D::PoseKey key(1); + double mu = 1000.0; + simulated2D::UnaryEqualityConstraint constraint(pt, key, mu); + + simulated2D::Config config1; + config1.insert(key, pt); + EXPECT(constraint.active(config1)); + EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); + EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); + + simulated2D::Config config2; + Point2 ptBad1(2.0, 2.0); + config2.insert(key, ptBad1); + EXPECT(constraint.active(config2)); + EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, unary_linearization ) { + Point2 pt(1.0, 2.0); + simulated2D::PoseKey key(1); + double mu = 1000.0; + simulated2D::UnaryEqualityConstraint constraint(pt, key, mu); + + simulated2D::Config config1; + config1.insert(key, pt); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); + GaussianFactor::shared_ptr expected1(new GaussianFactor(key, eye(2,2), zero(2), hard_model)); + EXPECT(assert_equal(*expected1, *actual1, tol)); + + simulated2D::Config config2; + Point2 ptBad(2.0, 2.0); + config2.insert(key, ptBad); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); + GaussianFactor::shared_ptr expected2(new GaussianFactor(key, eye(2,2), Vector_(2,-1.0,0.0), hard_model)); + EXPECT(assert_equal(*expected2, *actual2, tol)); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { + // create a single-node graph with a soft and hard constraint to + // ensure that the hard constraint overrides the soft constraint + Point2 truth_pt(1.0, 2.0); + simulated2D::PoseKey key(1); + double mu = 1000.0; + simulated2D::UnaryEqualityConstraint::shared_ptr constraint( + new simulated2D::UnaryEqualityConstraint(truth_pt, key, mu)); + + Point2 badPt(100.0, -200.0); + simulated2D::Prior::shared_ptr factor( + new simulated2D::Prior(badPt, soft_model, key)); + + shared_graph graph(new Graph()); + graph->push_back(constraint); + graph->push_back(factor); + + shared_config initConfig(new simulated2D::Config()); + initConfig->insert(key, badPt); + + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig); + simulated2D::Config expected; + expected.insert(key, truth_pt); + CHECK(assert_equal(expected, *actual, tol)); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, odo_basics ) { + Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); + simulated2D::PoseKey key1(1), key2(2); + double mu = 1000.0; + simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); + + simulated2D::Config config1; + config1.insert(key1, x1); + config1.insert(key2, x2); + EXPECT(constraint.active(config1)); + EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol)); + EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); + + simulated2D::Config config2; + Point2 x1bad(2.0, 2.0); + Point2 x2bad(2.0, 2.0); + config2.insert(key1, x1bad); + config2.insert(key2, x2bad); + EXPECT(constraint.active(config2)); + EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol)); + EXPECT_DOUBLES_EQUAL(2000.0, constraint.error(config2), tol); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, odo_linearization ) { + Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); + simulated2D::PoseKey key1(1), key2(2); + double mu = 1000.0; + simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); + + simulated2D::Config config1; + config1.insert(key1, x1); + config1.insert(key2, x2); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); + GaussianFactor::shared_ptr expected1( + new GaussianFactor(key1, -eye(2,2), key2, eye(2,2), zero(2), hard_model)); + EXPECT(assert_equal(*expected1, *actual1, tol)); + + simulated2D::Config config2; + Point2 x1bad(2.0, 2.0); + Point2 x2bad(2.0, 2.0); + config2.insert(key1, x1bad); + config2.insert(key2, x2bad); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); + GaussianFactor::shared_ptr expected2( + new GaussianFactor(key1, -eye(2,2), key2, eye(2,2), Vector_(2, 1.0, 1.0), hard_model)); + EXPECT(assert_equal(*expected2, *actual2, tol)); +} + +/* ************************************************************************* */ +TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { + // create a two-node graph, connected by an odometry constraint, with + // a hard prior on one variable, and a conflicting soft prior + // on the other variable - the constraints should override the soft constraint + Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); + simulated2D::PoseKey key1(1), key2(2); + + // hard prior on x1 + simulated2D::UnaryEqualityConstraint::shared_ptr constraint1( + new simulated2D::UnaryEqualityConstraint(truth_pt1, key1)); + + // soft prior on x2 + Point2 badPt(100.0, -200.0); + simulated2D::Prior::shared_ptr factor( + new simulated2D::Prior(badPt, soft_model, key2)); + + // odometry constraint + simulated2D::OdoEqualityConstraint::shared_ptr constraint2( + new simulated2D::OdoEqualityConstraint( + gtsam::between(truth_pt1, truth_pt2), key1, key2)); + + shared_graph graph(new Graph()); + graph->push_back(constraint1); + graph->push_back(constraint2); + graph->push_back(factor); + + shared_config initConfig(new simulated2D::Config()); + initConfig->insert(key1, Point2()); + initConfig->insert(key2, badPt); + + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig); + simulated2D::Config expected; + expected.insert(key1, truth_pt1); + expected.insert(key2, truth_pt2); + CHECK(assert_equal(expected, *actual, tol)); + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +