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); } +/* ************************************************************************* */ + +