diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index 1298c40ea..34f38e543 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -26,7 +26,6 @@ headers += NonlinearFactor.h sources += NonlinearOptimizer.cpp Ordering.cpp # Nonlinear constraints -headers += NonlinearConstraint.h headers += NonlinearEquality.h #---------------------------------------------------------------------------------------------------- diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h deleted file mode 100644 index f62ba498a..000000000 --- a/nonlinear/NonlinearConstraint.h +++ /dev/null @@ -1,548 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file NonlinearConstraint.h - * @brief Implements nonlinear constraints that can be linearized using - * direct linearization and solving through a quadratic merit function - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/** - * Base class for nonlinear constraints - * This allows for both equality and inequality constraints, - * where equality constraints are active all the time (even slightly - * nonzero constraint functions will still be active - inequality - * constraints should be sure to force to actual zero) - * - * NOTE: inequality constraints removed for now - * - * Nonlinear constraints evaluate their error as a part of a quadratic - * error function: ||h(x)-z||^2 + mu * ||c(x)|| where mu is a gain - * on the constraint function that should be made high enough to be - * significant - */ -template -class NonlinearConstraint : public NonlinearFactor { - -protected: - typedef NonlinearConstraint This; - typedef NonlinearFactor Base; - - double mu_; /// gain for quadratic merit function - size_t dim_; /// dimension of the constraint - -public: - - /** Constructor - sets the cost function and the lagrange multipliers - * @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): - Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)), dim_(dim) {} - virtual ~NonlinearConstraint() {} - - /** returns the gain mu */ - double mu() const { return mu_; } - - /** Print */ - virtual void print(const std::string& s = "") const=0; - - /** dimension of the constraint (number of rows) */ - size_t dim() const { return dim_; } - - /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& 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 Values& 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 Values& c) const = 0; - - /** - * active set check, defines what type of constraint this is - * - * In an inequality/bounding constraint, this active() returns true - * when the constraint is *NOT* fulfilled. - * @return true if the constraint is active - */ - virtual bool active(const Values& c) const=0; - - /** - * Linearizes around a given config - * @param config is the values structure - * @return a combined linear factor containing both the constraint and the constraint factor - */ - virtual boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const=0; -}; - - -/** - * A unary constraint that defaults to an equality constraint - */ -template -class NonlinearConstraint1 : public NonlinearConstraint { - -public: - typedef typename Key::Value X; - -protected: - typedef NonlinearConstraint1 This; - typedef NonlinearConstraint Base; - - /** key for the constrained variable */ - Key key_; - -public: - - /** - * Basic constructor - * @param key is the identifier for the variable constrained - * @param dim is the size of the constraint (p) - * @param mu is the gain for the factor - */ - NonlinearConstraint1(const Key& key, size_t dim, double mu = 1000.0) - : Base(dim, mu), key_(key) { - this->keys_.push_back(key); - } - virtual ~NonlinearConstraint1() {} - - /* 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; - } - - /** Check if two factors are equal. Note type is Factor and needs cast. */ - virtual bool equals(const NonlinearFactor& 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_); - } - - /** error function g(x), switched depending on whether the constraint is active */ - inline Vector unwhitenedError(const Values& x) const { - if (!active(x)) { - return zero(this->dim()); - } - const Key& j = key_; - const X& xj = x[j]; - return evaluateError(xj); - } - - /** Linearize from config */ - boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { - if (!active(x)) { - boost::shared_ptr factor; - return factor; - } - const X& xj = x[key_]; - Matrix A; - Vector b = - evaluateError(xj, A); - Index var = ordering[key_]; - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - return GaussianFactor::shared_ptr(new GaussianFactor(var, A, b, model)); - } - - /** g(x) with optional derivative - does not depend on active */ - virtual Vector evaluateError(const X& x, boost::optional H = - boost::none) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { - return Factor::shared_ptr(new Factor(ordering[key_])); - } -}; - -/** - * Unary Equality constraint - simply forces the value of active() to true - */ -template -class NonlinearEqualityConstraint1 : public NonlinearConstraint1 { - -public: - typedef typename Key::Value X; - -protected: - typedef NonlinearEqualityConstraint1 This; - typedef NonlinearConstraint1 Base; - -public: - NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0) - : Base(key, dim, mu) {} - virtual ~NonlinearEqualityConstraint1() {} - - /** Always active, so fixed value for active() */ - virtual bool active(const Values& c) const { return true; } -}; - -/** - * A binary constraint with arbitrary cost and jacobian functions - */ -template -class NonlinearConstraint2 : public NonlinearConstraint { - -public: - typedef typename Key1::Value X1; - typedef typename Key2::Value X2; - -protected: - typedef NonlinearConstraint2 This; - typedef NonlinearConstraint Base; - - /** keys for the constrained variables */ - Key1 key1_; - Key2 key2_; - -public: - - /** - * Basic constructor - * @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(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); - } - virtual ~NonlinearConstraint2() {} - - /* 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; - } - - /** Check if two factors are equal. Note type is Factor and needs cast. */ - virtual bool equals(const NonlinearFactor& 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_); - } - - /** error function g(x), switched depending on whether the constraint is active */ - inline Vector unwhitenedError(const Values& x) const { - if (!active(x)) { - return zero(this->dim()); - } - 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 */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) 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()); - Index var1 = ordering[j1], var2 = ordering[j2]; - if (var1 < var2) - GaussianFactor::shared_ptr(new GaussianFactor(var1, grad1, var2, grad2, g, model)); - else - GaussianFactor::shared_ptr(new GaussianFactor(var2, grad2, var1, grad1, g, model)); - } - - /** g(x) with optional derivative2 - does not depend on active */ - virtual Vector evaluateError(const X1& x1, const X2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { - const Index var1 = ordering[key1_], var2 = ordering[key2_]; - if(var1 < var2) - return Factor::shared_ptr(new Factor(var1, var2)); - else - return Factor::shared_ptr(new Factor(var2, var1)); - } -}; - -/** - * Binary Equality constraint - simply forces the value of active() to true - */ -template -class NonlinearEqualityConstraint2 : public NonlinearConstraint2 { - -public: - typedef typename Key1::Value X1; - typedef typename Key2::Value X2; - -protected: - typedef NonlinearEqualityConstraint2 This; - typedef NonlinearConstraint2 Base; - -public: - NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) - : Base(key1, key2, dim, mu) {} - virtual ~NonlinearEqualityConstraint2() {} - - - /** Always active, so fixed value for active() */ - virtual bool active(const Values& c) const { return true; } -}; - -/** - * A ternary constraint - */ -template -class NonlinearConstraint3 : public NonlinearConstraint { - -public: - typedef typename Key1::Value X1; - typedef typename Key2::Value X2; - typedef typename Key3::Value X3; - -protected: - typedef NonlinearConstraint3 This; - typedef NonlinearConstraint Base; - - /** keys for the constrained variables */ - Key1 key1_; - Key2 key2_; - Key3 key3_; - -public: - - /** - * Basic constructor - * @param key1 is the identifier for the first variable constrained - * @param key2 is the identifier for the second variable constrained - * @param key3 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 - */ - NonlinearConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, - size_t dim, double mu = 1000.0) : - Base(dim, mu), key1_(key1), key2_(key2), key3_(key3) { - this->keys_.push_back(key1); - this->keys_.push_back(key2); - this->keys_.push_back(key3); - } - virtual ~NonlinearConstraint3() {} - - /* print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearConstraint3 " << s << std::endl; - std::cout << "key1: " << (std::string) key1_ << std::endl; - std::cout << "key2: " << (std::string) key2_ << std::endl; - std::cout << "key3: " << (std::string) key3_ << std::endl; - std::cout << "mu: " << this->mu_ << std::endl; - } - - /** Check if two factors are equal. Note type is Factor and needs cast. */ - virtual bool equals(const NonlinearFactor& 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_) && (key3_ == p->key3_); - } - - /** error function g(x), switched depending on whether the constraint is active */ - inline Vector unwhitenedError(const Values& x) const { - if (!active(x)) { - return zero(this->dim()); - } - const Key1& j1 = key1_; - const Key2& j2 = key2_; - const Key3& j3 = key3_; - const X1& xj1 = x[j1]; - const X2& xj2 = x[j2]; - const X3& xj3 = x[j3]; - return evaluateError(xj1, xj2, xj3); - } - - /** Linearize from config */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - if (!active(c)) { - boost::shared_ptr factor; - return factor; - } - const Key1& j1 = key1_; const Key2& j2 = key2_; const Key3& j3 = key3_; - const X1& x1 = c[j1]; const X2& x2 = c[j2]; const X3& x3 = c[j3]; - Matrix A1, A2, A3; - Vector b = -1.0 * evaluateError(x1, x2, x3, A1, A2, A3); - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - Index var1 = ordering[j1], var2 = ordering[j2], var3 = ordering[j3]; - - // perform sorting - if(var1 < var2 && var2 < var3) - return GaussianFactor::shared_ptr( - new GaussianFactor(var1, A1, var2, A2, var3, A3, b, model)); - else if(var2 < var1 && var1 < var3) - return GaussianFactor::shared_ptr( - new GaussianFactor(var2, A2, var1, A1, var3, A3, b, model)); - else if(var1 < var3 && var3 < var2) - return GaussianFactor::shared_ptr( - new GaussianFactor(var1, A1, var3, A3, var2, A2, b, model)); - else if(var2 < var3 && var3 < var1) - return GaussianFactor::shared_ptr( - new GaussianFactor(var2, A2, var3, A3, var1, A1, b, model)); - else if(var3 < var1 && var1 < var2) - return GaussianFactor::shared_ptr( - new GaussianFactor(var3, A3, var1, A1, var2, A2, b, model)); - else - return GaussianFactor::shared_ptr( - new GaussianFactor(var3, A3, var2, A2, var1, A1, b, model)); - } - - /** g(x) with optional derivative3 - does not depend on active */ - virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { - const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; - if(var1 < var2 && var2 < var3) - return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_])); - else if(var2 < var1 && var1 < var3) - return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key2_], ordering[key3_])); - else if(var1 < var3 && var3 < var2) - return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key3_], ordering[key2_])); - else if(var2 < var3 && var3 < var1) - return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key3_], ordering[key1_])); - else if(var3 < var1 && var1 < var2) - return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key1_], ordering[key2_])); - else - return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key2_], ordering[key1_])); - } -}; - -/** - * Ternary Equality constraint - simply forces the value of active() to true - */ -template -class NonlinearEqualityConstraint3 : public NonlinearConstraint3 { - -public: - typedef typename Key1::Value X1; - typedef typename Key2::Value X2; - typedef typename Key3::Value X3; - -protected: - typedef NonlinearEqualityConstraint3 This; - typedef NonlinearConstraint3 Base; - -public: - NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, - size_t dim, double mu = 1000.0) - : Base(key1, key2, key3, dim, mu) {} - virtual ~NonlinearEqualityConstraint3() {} - - /** Always active, so fixed value for active() */ - virtual bool active(const Values& c) const { return true; } -}; - - -/** - * Simple unary equality constraint - fixes a value for a variable - */ -template -class NonlinearEquality1 : public NonlinearEqualityConstraint1 { - -public: - typedef typename Key::Value X; - -protected: - typedef NonlinearEqualityConstraint1 Base; - - X value_; /// fixed value for variable - -public: - - typedef boost::shared_ptr > shared_ptr; - - NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) - : Base(key1, X::Dim(), mu), value_(value) {} - virtual ~NonlinearEquality1() {} - - /** g(x) with optional derivative */ - Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { - const size_t p = X::Dim(); - if (H1) *H1 = eye(p); - return value_.logmap(x1); - } -}; - - -/** - * Simple binary equality constraint - this constraint forces two factors to - * be the same. This constraint requires the underlying type to a Lie type - */ -template -class NonlinearEquality2 : public NonlinearEqualityConstraint2 { -public: - typedef typename Key::Value X; - -protected: - typedef NonlinearEqualityConstraint2 Base; - -public: - - typedef boost::shared_ptr > shared_ptr; - - NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::Dim(), mu) {} - virtual ~NonlinearEquality2() {} - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - const size_t p = X::Dim(); - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - return x1.logmap(x2); - } -}; - -} diff --git a/nonlinear/tests/testConstraintOptimizer.cpp b/nonlinear/tests/testConstraintOptimizer.cpp deleted file mode 100644 index f8caed16d..000000000 --- a/nonlinear/tests/testConstraintOptimizer.cpp +++ /dev/null @@ -1,498 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testConstraintOptimizer.cpp - * @brief Tests the optimization engine for SQP and BFGS Quadratic programming techniques - * @author Alex Cunningham - */ - -/** IMPORTANT NOTE: this file is only compiled when LDL is available */ - -#include -#include - -#include -#include - -#include - -#include -#include - -#define GTSAM_MAGIC_KEY - -#include // for operator += -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] - VectorValues 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 g = boost::none, - boost::optional B = boost::none) { - double x1 = x(0), x2 = x(1); - if (g) *g = Vector_(2, 2.0*x1, 2.0*(x2-2.0)); - if (B) *B = 2.0 * eye(2,2); - return (x2-2)*(x2-2) + x1*x1; - } - - /** - * constraint function with gradient and hessian - * cx = 4*x1^2 + x2^2 - 1; - */ - Vector constraint(const Vector& x, boost::optional A = boost::none, - boost::optional B = boost::none) { - double x1 = x(0), x2 = x(1); - if (A) *A = Matrix_(2,1, 8.0*x1, 2.0*x2); - if (B) *B = Matrix_(2,2, - 8.0, 0.0, - 0.0, 2.0); - return Vector_(1, 4.0*x1*x1 + x2*x2 - 1.0); - } - - /** - * evaluates a penalty function at a given point - */ - double penalty(const Vector& x) { - double constraint_gain = 1.0; - return objective(x) + constraint_gain*sum(abs(constraint(x))); - } - - -} - -/* ************************************************************************* */ - -/** SQP example from SQP tutorial (real saddle point) */ -namespace sqp_example2 { - - /** - * objective function with gradient and hessian - * fx = (x2-2)^2 - x1^2; - */ - double objective(const Vector& x, boost::optional g = boost::none, - boost::optional B = boost::none) { - double x1 = x(0), x2 = x(1); - if (g) *g = Vector_(2, -2.0*x1, 2.0*(x2-2.0)); - if (B) *B = Matrix_(2,2, -2.0, 0.0, 0.0, 2.0); - return (x2-2)*(x2-2) - x1*x1; - } - - /** - * constraint function with gradient and hessian - * cx = 4*x1^2 + x2^2 - 1; - */ - Vector constraint(const Vector& x, boost::optional A = boost::none, - boost::optional B = boost::none) { - double x1 = x(0), x2 = x(1); - if (A) *A = Matrix_(2,1, 8.0*x1, 2.0*x2); - if (B) *B = Matrix_(2,2, - 8.0, 0.0, - 0.0, 2.0); - return Vector_(1, 4.0*x1*x1 + x2*x2 - 1.0); - } - - /** - * evaluates a penalty function at a given point - */ - double penalty(const Vector& x) { - double constraint_gain = 1.0; - return objective(x) + constraint_gain*sum(abs(constraint(x))); - } -} - -/* ************************************************************************* */ -TEST( matrix, SQP_simple_analytic ) { - using namespace sqp_example1; - - // parameters - double stepsize = 0.25; - size_t maxIt = 50; - - // initial conditions - Vector x0 = Vector_(2, 2.0, 4.0), - lam0 = Vector_(1, -0.5); - - // current state - Vector x = x0, lam = lam0; - - for (size_t i =0; i0) { - Vector Bis = Bi * step, - y = dfx - prev_dfx; - Bi = Bi + outer_prod(y, y) / inner_prod(y, step) - - outer_prod(Bis, Bis) / inner_prod(step, Bis); - } - prev_dfx = dfx; - - // solve subproblem - Vector delta, lambda; - boost::tie(delta, lambda) = solveCQP(Bi, -dcx, dfx, -cx); - - // update - step = stepsize * delta; - x = x + step; - lam = lambda; - } - - // verify - Vector expX = Vector_(2, 0.0, 1.0), - expLambda = Vector_(1, -1.0); - - CHECK(assert_equal(expX, x, 1e-4)); - CHECK(assert_equal(expLambda, lam, 1e-4)); -} - -/* ************************************************************************* */ -TEST( matrix, SQP_simple_bfgs1 ) { - using namespace sqp_example1; - - // parameters - size_t maxIt = 25; - - // initial conditions - Vector x0 = Vector_(2, 2.0, 4.0), - lam0 = Vector_(1, -0.5); - - // create a BFGSEstimator - BFGSEstimator hessian(2); - - // current state - Vector x = x0, lam = lam0; - Vector step; - - for (size_t i=0; i0) { - hessian.update(dfx, step); - } else { - hessian.update(dfx); - } - - // solve subproblem - Vector delta, lambda; - boost::tie(delta, lambda) = solveCQP(hessian.getB(), -dcx, dfx, -cx); -// if (i == 0) print(delta, "delta1"); - - // update - step = linesearch(x,delta,penalty); -// step = stepsize * delta; - x = x + step; - lam = lambda; - } - - // verify - Vector expX = Vector_(2, 0.0, 1.0), - expLambda = Vector_(1, -1.0); - - CHECK(assert_equal(expX, x, 1e-4)); - CHECK(assert_equal(expLambda, lam, 1e-4)); -} - -/* ************************************************************************* */ -TEST( matrix, SQP_simple_bfgs2 ) { - using namespace sqp_example2; - - // parameters - double stepsize = 0.25; - size_t maxIt = 50; - - // initial conditions - Vector x0 = Vector_(2, 2.0, 4.0), - lam0 = Vector_(1, -0.5); - - // create a BFGSEstimator - BFGSEstimator hessian(2); - - // current state - Vector x = x0, lam = lam0; - Vector step; - - for (size_t i=0; i0) { - hessian.update(dfx, step); - } else { - hessian.update(dfx); - } - - // solve subproblem - Vector delta, lambda; - boost::tie(delta, lambda) = solveCQP(hessian.getB(), -dcx, dfx, -cx); -// if (i == 0) print(delta, "delta2"); - - // update -// step = linesearch(x,delta,penalty); - step = stepsize * delta; - x = x + step; - lam = lambda; - } - - // verify - Vector expX = Vector_(2, 0.0, 1.0), - expLambda = Vector_(1, -1.0); - - // should determine the actual values for this one -// CHECK(assert_equal(expX, x, 1e-4)); -// CHECK(assert_equal(expLambda, lam, 1e-4)); -} - -/* ************************************************************************* */ -TEST( matrix, line_search ) { - using namespace sqp_example2; - - // initial conditions - Vector x0 = Vector_(2, 2.0, 4.0), - delta = Vector_(2, 0.85, -5.575); - - Vector actual = linesearch(x0,delta,penalty); - - // check that error goes down - double init_error = penalty(x0), - final_error = penalty(x0 + actual); - - //double actual_stepsize = dot(actual, delta)/dot(delta, delta); -// cout << "actual_stepsize: " << actual_stepsize << endl; - - CHECK(final_error <= init_error); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h deleted file mode 100644 index d6fb7ef1a..000000000 --- a/slam/BetweenConstraint.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file BetweenConstraint.h - * @brief Implements a constraint to force a between - * @author Alex Cunningham - */ - -#pragma once - -#include - -namespace gtsam { - - /** - * Binary between constraint - forces between to a given value - * This constraint requires the underlying type to a Lie type - */ - template - class BetweenConstraint : public NonlinearEqualityConstraint2 { - public: - typedef typename Key::Value X; - - protected: - typedef NonlinearEqualityConstraint2 Base; - - X measured_; /// fixed between value - - public: - - typedef boost::shared_ptr > shared_ptr; - - BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::Dim(), mu), measured_(measured) {} - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - X hx = x1.between(x2, H1, H2); - return measured_.logmap(hx); - } - - inline const X measured() const { - return measured_; - } - }; - -} // \namespace gtsam diff --git a/slam/BoundingConstraint.h b/slam/BoundingConstraint.h deleted file mode 100644 index 51de0407f..000000000 --- a/slam/BoundingConstraint.h +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file BoundingConstraint.h - * @brief Provides partially implemented constraints to implement bounds - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** - * Unary inequality constraint forcing a scalar to be - * greater/less than a fixed threshold. The function - * will need to have its value function implemented to return - * a scalar for comparison. - */ - template - struct BoundingConstraint1: public NonlinearConstraint1 { - typedef typename Key::Value X; - typedef NonlinearConstraint1 Base; - typedef boost::shared_ptr > shared_ptr; - - double threshold_; - bool isGreaterThan_; /// flag for greater/less than - - BoundingConstraint1(const Key& key, double threshold, - bool isGreaterThan, double mu = 1000.0) : - Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) { - } - - inline double threshold() const { return threshold_; } - inline bool isGreaterThan() const { return isGreaterThan_; } - - /** - * function producing a scalar value to compare to the threshold - * Must have optional argument for derivative with 1xN matrix, where - * N = X::dim() - */ - virtual double value(const X& x, boost::optional H = - boost::none) const = 0; - - /** active when constraint *NOT* met */ - bool active(const Cfg& c) const { - // note: still active at equality to avoid zigzagging - double x = value(c[this->key_]); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; - } - - Vector evaluateError(const X& x, boost::optional H = - boost::none) const { - Matrix D; - double error = value(x, D) - threshold_; - if (H) *H = (isGreaterThan_) ? D : -1.0 * D; - return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error); - } - }; - - /** - * Binary scalar inequality constraint, with a similar value() function - * to implement for specific systems - */ - template - struct BoundingConstraint2: public NonlinearConstraint2 { - typedef typename Key1::Value X1; - typedef typename Key2::Value X2; - - typedef NonlinearConstraint2 Base; - typedef boost::shared_ptr > shared_ptr; - - double threshold_; - bool isGreaterThan_; /// flag for greater/less than - - BoundingConstraint2(const Key1& key1, const Key2& key2, double threshold, - bool isGreaterThan, double mu = 1000.0) - : Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {} - - inline double threshold() const { return threshold_; } - inline bool isGreaterThan() const { return isGreaterThan_; } - - /** - * function producing a scalar value to compare to the threshold - * Must have optional argument for derivatives) - */ - virtual double value(const X1& x1, const X2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const = 0; - - /** active when constraint *NOT* met */ - bool active(const Cfg& c) const { - // note: still active at equality to avoid zigzagging - double x = value(c[this->key1_], c[this->key2_]); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; - } - - Vector evaluateError(const X1& x1, const X2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - Matrix D1, D2; - double error = value(x1, x2, D1, D2) - threshold_; - if (H1) *H1 = (isGreaterThan_) ? D1 : -1.0 * D1; - if (H2) *H2 = (isGreaterThan_) ? D2 : -1.0 * D2; - return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error); - } - }; - -} // \namespace gtsam diff --git a/slam/LinearApproxFactor-inl.h b/slam/LinearApproxFactor-inl.h deleted file mode 100644 index 1b48320fa..000000000 --- a/slam/LinearApproxFactor-inl.h +++ /dev/null @@ -1,110 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LinearApproxFactor.h - * @brief A dummy factor that allows a linear factor to act as a nonlinear factor - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -LinearApproxFactor::LinearApproxFactor( - GaussianFactor::shared_ptr lin_factor, const Ordering& ordering, const Values& lin_points) -: Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())), - b_(lin_factor->getb()), model_(lin_factor->get_model()), lin_points_(lin_points) -{ - BOOST_FOREACH(const Ordering::Map::value_type& p, ordering) { - Symbol key = p.first; - Index var = p.second; - - // check if actually in factor - Factor::const_iterator it = lin_factor->find(var); - if (it != lin_factor->end()) { - // store matrix - Matrix A = lin_factor->getA(it); - matrices_.insert(make_pair(key, A)); - - // store keys - nonlinearKeys_.push_back(Key(key.index())); - this->keys_.push_back(key); - } - } -} - -/* ************************************************************************* */ -template -Vector LinearApproxFactor::unwhitenedError(const Values& c) const { - // extract the points in the new config - Vector ret = - b_; - - BOOST_FOREACH(const Key& key, nonlinearKeys_) { - X newPt = c[key], linPt = lin_points_[key]; - Vector d = linPt.logmap(newPt); - const Matrix& A = matrices_.at(Symbol(key)); - ret += prod(A, d); - } - - return ret; -} - -/* ************************************************************************* */ -template -boost::shared_ptr -LinearApproxFactor::linearize(const Values& c, const Ordering& ordering) const { - - // sort by varid - only known at linearization time - typedef std::map VarMatrixMap; - VarMatrixMap sorting_terms; - BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) - sorting_terms.insert(std::make_pair(ordering[p.first], p.second)); - - // move into terms - std::vector > terms; - BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms) - terms.push_back(p); - - return boost::shared_ptr(new GaussianFactor(terms, b_, model_)); -} - -/* ************************************************************************* */ -template -Factor::shared_ptr -LinearApproxFactor::symbolic(const Ordering& ordering) const { - std::vector key_ids(this->keys_.size()); - size_t i=0; - BOOST_FOREACH(const Symbol& key, this->keys_) - key_ids[i++] = ordering[key]; - std::sort(key_ids.begin(), key_ids.end()); - return boost::shared_ptr(new Factor(key_ids.begin(), key_ids.end())); -} - -/* ************************************************************************* */ -template -void LinearApproxFactor::print(const std::string& s) const { - LinearApproxFactor::Base::print(s); - BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) { - gtsam::print(p.second, (std::string) p.first); - } - gtsam::print(b_, std::string("b")); - model_->print("model"); -} - -} // \namespace gtsam diff --git a/slam/LinearApproxFactor.h b/slam/LinearApproxFactor.h deleted file mode 100644 index dceb2dd8f..000000000 --- a/slam/LinearApproxFactor.h +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LinearApproxFactor.h - * @brief A dummy factor that allows a linear factor to act as a nonlinear factor - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * A dummy factor that takes a linearized factor and inserts it into - * a nonlinear graph. This version uses exactly one type of variable. - */ -template -class LinearApproxFactor : public NonlinearFactor { - -public: - /** type for the variable */ - typedef typename Key::Value X; - - /** base type */ - typedef NonlinearFactor Base; - - /** shared pointer for convenience */ - typedef boost::shared_ptr > shared_ptr; - - /** typedefs for key vectors */ - typedef std::vector KeyVector; - -protected: - /** hold onto the factor itself */ -// GaussianFactor::shared_ptr lin_factor_; - // store components of a linear factor that can be reordered - typedef std::map SymbolMatrixMap; - SymbolMatrixMap matrices_; - Vector b_; - SharedDiagonal model_; - - /** linearization points for error calculation */ - Values lin_points_; - - /** keep keys for the factor */ - KeyVector nonlinearKeys_; - - /** - * use this for derived classes with keys that don't copy easily - */ - LinearApproxFactor(size_t dim, const Values& lin_points) - : Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {} - -public: - - /** - * use this constructor when starting with nonlinear keys - * - * Note that you need to have the ordering used to construct the factor - * initially in order to find the actual keys - */ - LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, - const Ordering& ordering, const Values& lin_points); - - virtual ~LinearApproxFactor() {} - - /** Vector of errors, unwhitened ! */ - virtual Vector unwhitenedError(const Values& c) const; - - /** - * linearize to a GaussianFactor - * Reconstructs the linear factor from components to ensure that - * the ordering is correct - */ - virtual boost::shared_ptr linearize( - const Values& c, const Ordering& ordering) const; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - Factor::shared_ptr symbolic(const Ordering& ordering) const; - - /** get access to nonlinear keys */ - KeyVector nonlinearKeys() const { return nonlinearKeys_; } - - /** override print function */ - virtual void print(const std::string& s="") const; - - /** access to b vector of gaussian */ - Vector get_b() const { return b_; } -}; - -} // \namespace gtsam diff --git a/slam/Makefile.am b/slam/Makefile.am index 9fb78ac17..7d0563e44 100644 --- a/slam/Makefile.am +++ b/slam/Makefile.am @@ -31,12 +31,6 @@ check_PROGRAMS += tests/testSimulated3D # Pose SLAM headers headers += BetweenFactor.h PriorFactor.h -# General constraints -headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h - -# Utility factors -headers += LinearApproxFactor.h LinearApproxFactor-inl.h - # 2D Pose SLAM sources += pose2SLAM.cpp dataset.cpp #sources += Pose2SLAMOptimizer.cpp diff --git a/slam/TransformConstraint.h b/slam/TransformConstraint.h deleted file mode 100644 index 591896892..000000000 --- a/slam/TransformConstraint.h +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file TransformConstraint.h - * @brief A constraint for combining graphs by common landmarks and a transform node - * @author Alex Cunningham - */ - -#pragma once - -#include - -namespace gtsam { - -/** - * A constraint between two landmarks in separate maps - * Templated on: - * Values : The overall config - * PKey : Key of landmark being constrained - * Point : Type of landmark - * TKey : Key of transform used - * Transform : Transform variable class - * - * The Point and Transform concepts must be Lie types, and the transform - * relationship "Point = transform_from(Transform, Point)" must exist. - * - * This base class should be specialized to implement the cost function for - * specific classes of landmarks - */ -template -class TransformConstraint : public NonlinearEqualityConstraint3 { - -public: - typedef typename PKey::Value Point; - typedef typename TKey::Value Transform; - - typedef NonlinearEqualityConstraint3 Base; - typedef TransformConstraint This; - - /** - * General constructor with all of the keys to variables in the map - * Extracts everything necessary from the key types - */ - TransformConstraint(const PKey& foreignKey, const TKey& transKey, const PKey& localKey, double mu = 1000.0) - : Base(foreignKey, transKey, localKey, Point().dim(), mu) {} - - virtual ~TransformConstraint(){} - - /** Combined cost and derivative function using boost::optional */ - virtual Vector evaluateError(const Point& foreign, const Transform& trans, const Point& local, - boost::optional Dforeign = boost::none, - boost::optional Dtrans = boost::none, - boost::optional Dlocal = boost::none) const { - Point newlocal = trans.transform_from(foreign, Dtrans, Dforeign); - if (Dlocal) { - Point dummy; - *Dlocal = -1* eye(dummy.dim()); - } - return local.logmap(newlocal); - } -}; -} // \namespace gtsam diff --git a/tests/Makefile.am b/tests/Makefile.am index 57609f05b..128427a3b 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -19,10 +19,6 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG check_PROGRAMS += testNonlinearOptimizer check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph check_PROGRAMS += testTupleValues -#check_PROGRAMS += testNonlinearEqualityConstraint -#check_PROGRAMS += testBoundingConstraint -check_PROGRAMS += testTransformConstraint -check_PROGRAMS += testLinearApproxFactor # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp deleted file mode 100644 index 8f818ffab..000000000 --- a/tests/testBoundingConstraint.cpp +++ /dev/null @@ -1,282 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testBoundingConstraint.cpp - * @brief test of nonlinear inequality constraints on scalar bounds - * @author Alex Cunningham - */ - -#include - -#include -#include -#include - -namespace iq2D = gtsam::simulated2D::inequality_constraints; -using namespace std; -using namespace gtsam; - -static const double tol = 1e-5; - -SharedDiagonal soft_model2 = noiseModel::Unit::Create(2); -SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1); -SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); - -typedef NonlinearFactorGraph Graph; -typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; - -// some simple inequality constraints -simulated2D::PoseKey key(1); -double mu = 10.0; -// greater than -iq2D::PoseXInequality constraint1(key, 1.0, true, mu); -iq2D::PoseYInequality constraint2(key, 2.0, true, mu); - -// less than -iq2D::PoseXInequality constraint3(key, 1.0, false, mu); -iq2D::PoseYInequality constraint4(key, 2.0, false, mu); - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_basics_inactive1 ) { - Point2 pt1(2.0, 3.0); - simulated2D::Values config; - config.insert(key, pt1); - EXPECT(!constraint1.active(config)); - EXPECT(!constraint2.active(config)); - EXPECT_DOUBLES_EQUAL(1.0, constraint1.threshold(), tol); - EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol); - EXPECT(constraint1.isGreaterThan()); - EXPECT(constraint2.isGreaterThan()); - EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); - EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); - EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol)); - EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); - EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_basics_inactive2 ) { - Point2 pt2(-2.0, -3.0); - simulated2D::Values config; - config.insert(key, pt2); - EXPECT(!constraint3.active(config)); - EXPECT(!constraint4.active(config)); - EXPECT_DOUBLES_EQUAL(1.0, constraint3.threshold(), tol); - EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol); - EXPECT(!constraint3.isGreaterThan()); - EXPECT(!constraint4.isGreaterThan()); - EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol)); - EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol)); - EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); - EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_basics_active1 ) { - Point2 pt2(-2.0, -3.0); - simulated2D::Values config; - config.insert(key, pt2); - EXPECT(constraint1.active(config)); - EXPECT(constraint2.active(config)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol)); - EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol); - EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_basics_active2 ) { - Point2 pt1(2.0, 3.0); - simulated2D::Values config; - config.insert(key, pt1); - EXPECT(constraint3.active(config)); - EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol)); - EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol); - EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_linearization_inactive) { - Point2 pt1(2.0, 3.0); - simulated2D::Values config1; - config1.insert(key, pt1); - GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1); - GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1); - EXPECT(!actual1); - EXPECT(!actual2); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_linearization_active) { - Point2 pt2(-2.0, -3.0); - simulated2D::Values config2; - config2.insert(key, pt2); - GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); - GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - GaussianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); - GaussianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); - EXPECT(assert_equal(expected1, *actual1, tol)); - EXPECT(assert_equal(expected2, *actual2, tol)); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_simple_optimization1) { - // create a single-node graph with a soft and hard constraint to - // ensure that the hard constraint overrides the soft constraint - Point2 goal_pt(1.0, 2.0); - Point2 start_pt(0.0, 1.0); - - shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); - graph->add(iq2D::PoseXInequality(x1, 1.0, true)); - graph->add(iq2D::PoseYInequality(x1, 2.0, true)); - graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - - shared_values initValues(new simulated2D::Values()); - initValues->insert(x1, start_pt); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues, Optimizer::SILENT); - simulated2D::Values expected; - expected.insert(x1, goal_pt); - CHECK(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, unary_simple_optimization2) { - // create a single-node graph with a soft and hard constraint to - // ensure that the hard constraint overrides the soft constraint - Point2 goal_pt(1.0, 2.0); - Point2 start_pt(2.0, 3.0); - - shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); - graph->add(iq2D::PoseXInequality(x1, 1.0, false)); - graph->add(iq2D::PoseYInequality(x1, 2.0, false)); - graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - - shared_values initValues(new simulated2D::Values()); - initValues->insert(x1, start_pt); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues, Optimizer::SILENT); - simulated2D::Values expected; - expected.insert(x1, goal_pt); - CHECK(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, MaxDistance_basics) { - simulated2D::PoseKey key1(1), key2(2); - Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); - iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); - EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); - EXPECT(!rangeBound.isGreaterThan()); - EXPECT(rangeBound.dim() == 1); - - EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1))); - EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); - EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); - - simulated2D::Values config1; - config1.insert(key1, pt1); - config1.insert(key2, pt1); - EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); - EXPECT(!rangeBound.linearize(config1)); - EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); - - config1.update(key2, pt2); - EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); - EXPECT(!rangeBound.linearize(config1)); - EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); - - config1.update(key2, pt3); - EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); - EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); - - config1.update(key2, pt4); - EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1))); - EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, MaxDistance_simple_optimization) { - - Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); - simulated2D::PoseKey x1(1), x2(2); - - Graph graph; - graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); - graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2)); - graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0)); - - simulated2D::Values initial_state; - initial_state.insert(x1, pt1); - initial_state.insert(x2, pt2_init); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initial_state); - - simulated2D::Values expected; - expected.insert(x1, pt1); - expected.insert(x2, pt2_goal); - - EXPECT(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -TEST( testBoundingConstraint, avoid_demo) { - - simulated2D::PoseKey x1(1), x2(2), x3(3); - simulated2D::PointKey l1(1); - double radius = 1.0; - Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); - Point2 odo(2.0, 0.0); - - Graph graph; - graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1)); - graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2)); - graph.add(iq2D::LandmarkAvoid(x2, l1, radius)); - graph.add(simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1)); - graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3)); - graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3)); - - simulated2D::Values init, expected; - init.insert(x1, x1_pt); - init.insert(x3, x3_pt); - init.insert(l1, l1_pt); - expected = init; - init.insert(x2, x2_init); - expected.insert(x2, x2_goal); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init); - - EXPECT(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - diff --git a/tests/testLinearApproxFactor.cpp b/tests/testLinearApproxFactor.cpp deleted file mode 100644 index 866c53889..000000000 --- a/tests/testLinearApproxFactor.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testLinearApproxFactor.cpp - * @brief tests for dummy factor that contains a linear factor - * @author Alex Cunningham - */ - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -typedef LinearApproxFactor ApproxFactor; - -/* ************************************************************************* */ -TEST ( LinearApproxFactor, basic ) { - Symbol key1('l', 1); - Matrix A1 = eye(2); - Vector b = repeat(2, 1.2); - SharedDiagonal model = noiseModel::Unit::Create(2); - GaussianFactor::shared_ptr lin_factor(new GaussianFactor(0, A1, b, model)); - Ordering ordering; - ordering.push_back(key1); - planarSLAM::Values lin_points; - planarSLAM::PointKey PKey(1); - Point2 point(1.0, 2.0); - lin_points.insert(PKey, point); - ApproxFactor f1(lin_factor, ordering, lin_points); - - EXPECT(f1.size() == 1); - EXPECT(assert_equal(key1, f1.keys().front())); - EXPECT(assert_equal(b, f1.get_b())); - - planarSLAM::Values config; - config.insert(PKey, Point2(2.0, 3.0)); - GaussianFactor::shared_ptr actual = f1.linearize(config, ordering); - - EXPECT(assert_equal(Vector_(2, -0.2, -0.2), f1.unwhitenedError(config))); - - // Check the linearization - EXPECT(assert_equal(*lin_factor, *actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp deleted file mode 100644 index af7276b16..000000000 --- a/tests/testNonlinearEqualityConstraint.cpp +++ /dev/null @@ -1,363 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testNonlinearEqualityConstraint.cpp - * @author Alex Cunningham - */ - -#include - -#include -#include -#include -#include - -namespace eq2D = gtsam::simulated2D::equality_constraints; - -using namespace std; -using namespace gtsam; - -static const double tol = 1e-5; - -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_values; -typedef NonlinearOptimizer Optimizer; - -/* ************************************************************************* */ -TEST( testNonlinearEqualityConstraint, unary_basics ) { - Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); - double mu = 1000.0; - eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - - simulated2D::Values 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::Values 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; - eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - - simulated2D::Values 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::Values 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; - eq2D::UnaryEqualityConstraint::shared_ptr constraint( - new eq2D::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_values initValues(new simulated2D::Values()); - initValues->insert(key, badPt); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values 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; - eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - - simulated2D::Values 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::Values 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; - eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - - simulated2D::Values 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::Values 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 - eq2D::UnaryEqualityConstraint::shared_ptr constraint1( - new eq2D::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 - eq2D::OdoEqualityConstraint::shared_ptr constraint2( - new eq2D::OdoEqualityConstraint( - truth_pt1.between(truth_pt2), key1, key2)); - - shared_graph graph(new Graph()); - graph->push_back(constraint1); - graph->push_back(constraint2); - graph->push_back(factor); - - shared_values initValues(new simulated2D::Values()); - initValues->insert(key1, Point2()); - initValues->insert(key2, badPt); - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; - expected.insert(key1, truth_pt1); - expected.insert(key2, truth_pt2); - CHECK(assert_equal(expected, *actual, tol)); -} - -/* ********************************************************************* */ -TEST (testNonlinearEqualityConstraint, two_pose ) { - /* - * Determining a ground truth linear system - * with two poses seeing one landmark, with each pose - * constrained to a particular value - */ - - shared_graph graph(new Graph()); - - 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); - graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); - graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); - - Point2 z1(0.0, 5.0); - SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph->add(simulated2D::Measurement(z1, sigma, x1,l1)); - - Point2 z2(-4.0, 0.0); - graph->add(simulated2D::Measurement(z2, sigma, x2,l2)); - - graph->add(eq2D::PointEqualityConstraint(l1, l2)); - - shared_values initialEstimate(new simulated2D::Values()); - 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 - - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - - simulated2D::Values 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, *actual, 1e-5)); -} - -/* ********************************************************************* */ -TEST (testNonlinearEqualityConstraint, map_warp ) { - // get a graph - shared_graph graph(new Graph()); - - // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); - - // constant constraint on x1 - Point2 pose1(1.0, 1.0); - graph->add(eq2D::UnaryEqualityConstraint(pose1, x1)); - - SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1); - - // measurement from x1 to l1 - Point2 z1(0.0, 5.0); - graph->add(simulated2D::Measurement(z1, sigma, x1, l1)); - - // measurement from x2 to l2 - Point2 z2(-4.0, 0.0); - graph->add(simulated2D::Measurement(z2, sigma, x2, l2)); - - // equality constraint between l1 and l2 - graph->add(eq2D::PointEqualityConstraint(l1, l2)); - - // create an initial estimate - shared_values initialEstimate(new simulated2D::Values()); - 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 - - // optimize - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - - simulated2D::Values 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, tol)); -} - -// 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)); - -// typedefs for visual SLAM example -typedef visualSLAM::Values VValues; -typedef boost::shared_ptr shared_vconfig; -typedef visualSLAM::Graph VGraph; -typedef NonlinearOptimizer VOptimizer; - -// factors for visual slam -typedef NonlinearEquality2 Point3Equality; - -/* ********************************************************************* */ -TEST (testNonlinearEqualityConstraint, stereo_constrained ) { - - // 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 - - // keys - visualSLAM::PoseKey x1(1), x2(2); - visualSLAM::PointKey l1(1), l2(2); - - // create graph - VGraph::shared_graph graph(new VGraph()); - - // create equality constraints for poses - graph->addPoseConstraint(1, camera1.pose()); - graph->addPoseConstraint(2, camera2.pose()); - - // create factors - SharedDiagonal vmodel = noiseModel::Unit::Create(3); - graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK); - graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK); - - // add equality constraint - graph->add(Point3Equality(l1, l2)); - - // create initial data - Point3 landmark1(0.5, 5.0, 0.0); - Point3 landmark2(1.5, 5.0, 0.0); - - shared_vconfig initValues(new VValues()); - initValues->insert(x1, pose1); - initValues->insert(x2, pose2); - initValues->insert(l1, landmark1); - initValues->insert(l2, landmark2); - - // optimize - VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); - - // create config - VValues truthValues; - truthValues.insert(x1, camera1.pose()); - truthValues.insert(x2, camera2.pose()); - truthValues.insert(l1, landmark); - truthValues.insert(l2, landmark); - - // check if correct - CHECK(assert_equal(truthValues, *actual, 1e-5)); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - - diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp deleted file mode 100644 index 0ccc5607b..000000000 --- a/tests/testTransformConstraint.cpp +++ /dev/null @@ -1,276 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file testTransformConstraint.cpp - * @author Alex Cunningham - */ - -#include - -#include - -#include - -#include - -#include -#include - -#include -#include - -// implementations -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost; - -typedef TypedSymbol PoseKey; -typedef TypedSymbol PointKey; -typedef TypedSymbol TransformKey; - -typedef LieValues PoseValues; -typedef LieValues PointValues; -typedef LieValues TransformValues; - -typedef TupleValues3< PoseValues, PointValues, TransformValues > DDFValues; -typedef NonlinearFactorGraph DDFGraph; -typedef NonlinearOptimizer Optimizer; - -typedef NonlinearEquality PoseConstraint; -typedef NonlinearEquality PointConstraint; -typedef NonlinearEquality TransformPriorConstraint; - -typedef TransformConstraint PointTransformConstraint; - -PointKey lA1(1), lA2(2), lB1(3); -TransformKey t1(1); - -/* ************************************************************************* */ -TEST( TransformConstraint, equals ) { - PointTransformConstraint c1(lB1, t1, lA1), - c2(lB1, t1, lA1), - c3(lB1, t1, lA2); - - CHECK(assert_equal(c1, c1)); - CHECK(assert_equal(c1, c2)); - CHECK(!c1.equals(c3)); -} - -/* ************************************************************************* */ -LieVector evaluateError_(const PointTransformConstraint& c, - const Point2& global, const Pose2& trans, const Point2& local) { - return LieVector(c.evaluateError(global, trans, local)); -} -TEST( TransformConstraint, jacobians ) { - - // from examples below - Point2 local(2.0, 3.0), global(-1.0, 2.0); - Pose2 trans(1.5, 2.5, 0.3); - - PointTransformConstraint tc(lA1, t1, lB1); - Matrix actualDT, actualDL, actualDF; - tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); - - Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), - global, trans, local, 1e-5); - numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), - global, trans, local, 1e-5); - numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), - global, trans, local, 1e-5); - - CHECK(assert_equal(numericalDF, actualDF)); - CHECK(assert_equal(numericalDL, actualDL)); - CHECK(assert_equal(numericalDT, actualDT)); -} - -/* ************************************************************************* */ -TEST( TransformConstraint, jacobians_zero ) { - - // get values that are ideal - Pose2 trans(2.0, 3.0, 0.0); - Point2 global(5.0, 6.0); - Point2 local = trans.transform_from(global); - - PointTransformConstraint tc(lA1, t1, lB1); - Vector actCost = tc.evaluateError(global, trans, local), - expCost = zero(2); - CHECK(assert_equal(expCost, actCost, 1e-5)); - - Matrix actualDT, actualDL, actualDF; - tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); - - Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), - global, trans, local, 1e-5); - numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), - global, trans, local, 1e-5); - numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), - global, trans, local, 1e-5); - - CHECK(assert_equal(numericalDF, actualDF)); - CHECK(assert_equal(numericalDL, actualDL)); - CHECK(assert_equal(numericalDT, actualDT)); -} - -/* ************************************************************************* */ -TEST( TransformConstraint, converge_trans ) { - - // initial points - Point2 local1(2.0, 2.0), local2(4.0, 5.0), - global1(-1.0, 5.0), global2(2.0, 3.0); - Pose2 transIdeal(7.0, 3.0, M_PI/2); - - // verify direction - CHECK(assert_equal(local1, transIdeal.transform_from(global1))); - CHECK(assert_equal(local2, transIdeal.transform_from(global2))); - - // choose transform -// Pose2 trans = transIdeal; // ideal - works -// Pose2 trans = transIdeal * Pose2(0.1, 1.0, 0.00); // translation - works -// Pose2 trans = transIdeal * Pose2(10.1, 1.0, 0.00); // large translation - works -// Pose2 trans = transIdeal * Pose2(0.0, 0.0, 0.1); // small rotation - works - Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 1.3); // combined - works -// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails - - // keys - PointKey localK1(1), localK2(2), - globalK1(3), globalK2(4); - TransformKey transK(1); - - DDFGraph graph; - - graph.add(PointTransformConstraint(globalK1, transK, localK1)); - graph.add(PointTransformConstraint(globalK2, transK, localK2)); - - // hard constraints on points - double error_gain = 1000.0; - graph.add(PointConstraint(localK1, local1, error_gain)); - graph.add(PointConstraint(localK2, local2, error_gain)); - graph.add(PointConstraint(globalK1, global1, error_gain)); - graph.add(PointConstraint(globalK2, global2, error_gain)); - - // create initial estimate - DDFValues init; - init.insert(localK1, local1); - init.insert(localK2, local2); - init.insert(globalK1, global1); - init.insert(globalK2, global2); - init.insert(transK, trans); - - // optimize - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init); - - DDFValues expected; - expected.insert(localK1, local1); - expected.insert(localK2, local2); - expected.insert(globalK1, global1); - expected.insert(globalK2, global2); - expected.insert(transK, transIdeal); - - CHECK(assert_equal(expected, *actual, 1e-4)); -} - -/* ************************************************************************* */ -TEST( TransformConstraint, converge_local ) { - - // initial points - Point2 global(-1.0, 2.0); -// Pose2 trans(1.5, 2.5, 0.3); // original -// Pose2 trans(1.5, 2.5, 1.0); // larger rotation - Pose2 trans(1.5, 2.5, 3.1); // significant rotation - - Point2 idealLocal = trans.transform_from(global); - - // perturb the initial estimate -// Point2 local = idealLocal; // Ideal case - works -// Point2 local = idealLocal + Point2(1.0, 0.0); // works - Point2 local = idealLocal + Point2(-10.0, 10.0); // works - - - // keys - PointKey localK(1), globalK(2); - TransformKey transK(1); - - DDFGraph graph; - double error_gain = 1000.0; - graph.add(PointTransformConstraint(globalK, transK, localK)); - graph.add(PointConstraint(globalK, global, error_gain)); - graph.add(TransformPriorConstraint(transK, trans, error_gain)); - - // create initial estimate - DDFValues init; - init.insert(localK, local); - init.insert(globalK, global); - init.insert(transK, trans); - - // optimize - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init); - - CHECK(assert_equal(idealLocal, actual->at(localK), 1e-5)); -} - -/* ************************************************************************* */ -TEST( TransformConstraint, converge_global ) { - - // initial points - Point2 local(2.0, 3.0); -// Pose2 trans(1.5, 2.5, 0.3); // original -// Pose2 trans(1.5, 2.5, 1.0); // larger rotation - Pose2 trans(1.5, 2.5, 3.1); // significant rotation - - Point2 idealForeign = trans.inverse().transform_from(local); - - // perturb the initial estimate -// Point2 global = idealForeign; // Ideal - works -// Point2 global = idealForeign + Point2(1.0, 0.0); // simple - works - Point2 global = idealForeign + Point2(10.0, -10.0); // larger - works - - // keys - PointKey localK(1), globalK(2); - TransformKey transK(1); - - DDFGraph graph; - double error_gain = 1000.0; - graph.add(PointTransformConstraint(globalK, transK, localK)); - graph.add(PointConstraint(localK, local, error_gain)); - graph.add(TransformPriorConstraint(transK, trans, error_gain)); - - // create initial estimate - DDFValues init; - init.insert(localK, local); - init.insert(globalK, global); - init.insert(transK, trans); - - // optimize - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init); - - // verify - CHECK(assert_equal(idealForeign, actual->at(globalK), 1e-5)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - -