diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 371c03dd8..9bf1d820a 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -31,7 +31,7 @@ check_PROGRAMS += tests/testKey tests/testOrdering headers += NonlinearISAM.h NonlinearISAM-inl.h # Nonlinear constraints -headers += NonlinearEquality.h +headers += NonlinearEquality.h NonlinearConstraint.h #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h new file mode 100644 index 000000000..3e581f733 --- /dev/null +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -0,0 +1,670 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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; + + /** default constructor to allow for serialization */ + NonlinearConstraint() {} + + double mu_; /// gain for quadratic merit function + +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)) {} + virtual ~NonlinearConstraint() {} + + /** returns the gain mu */ + double mu() const { return mu_; } + + /** Print */ + virtual void print(const std::string& s = "") const=0; + + /** 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_ * error_vector.dot(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; + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(mu_); + } +}; + + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearConstraint1() {} + + /** 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 JacobianFactor(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 IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { + return IndexFactor::shared_ptr(new IndexFactor(ordering[key_])); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(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; + + /** default constructor to allow for serialization */ + NonlinearEqualityConstraint1() {} + +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; } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint1", + boost::serialization::base_object(*this)); + } + +}; + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearConstraint2() {} + + /** 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) + return GaussianFactor::shared_ptr(new JacobianFactor(var1, grad1, var2, grad2, g, model)); + else + return GaussianFactor::shared_ptr(new JacobianFactor(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 IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { + const Index var1 = ordering[key1_], var2 = ordering[key2_]; + if(var1 < var2) + return IndexFactor::shared_ptr(new IndexFactor(var1, var2)); + else + return IndexFactor::shared_ptr(new IndexFactor(var2, var1)); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + } +}; + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearEqualityConstraint2() {} + +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; } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint2", + boost::serialization::base_object(*this)); + } +}; + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearConstraint3() {} + + /** 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 JacobianFactor(var1, A1, var2, A2, var3, A3, b, model)); + else if(var2 < var1 && var1 < var3) + return GaussianFactor::shared_ptr( + new JacobianFactor(var2, A2, var1, A1, var3, A3, b, model)); + else if(var1 < var3 && var3 < var2) + return GaussianFactor::shared_ptr( + new JacobianFactor(var1, A1, var3, A3, var2, A2, b, model)); + else if(var2 < var3 && var3 < var1) + return GaussianFactor::shared_ptr( + new JacobianFactor(var2, A2, var3, A3, var1, A1, b, model)); + else if(var3 < var1 && var1 < var2) + return GaussianFactor::shared_ptr( + new JacobianFactor(var3, A3, var1, A1, var2, A2, b, model)); + else + return GaussianFactor::shared_ptr( + new JacobianFactor(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 IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { + const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; + if(var1 < var2 && var2 < var3) + return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_])); + else if(var2 < var1 && var1 < var3) + return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key1_], ordering[key3_])); + else if(var1 < var3 && var3 < var2) + return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key3_], ordering[key2_])); + else if(var2 < var3 && var3 < var1) + return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key3_], ordering[key1_])); + else if(var3 < var1 && var1 < var2) + return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key1_], ordering[key2_])); + else + return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key2_], ordering[key1_])); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(key3_); + } +}; + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearEqualityConstraint3() {} + +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; } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint3", + boost::serialization::base_object(*this)); + } +}; + + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearEquality1() {} + + 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); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearEqualityConstraint1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(value_); + } +}; + + +/** + * 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; + + /** default constructor to allow for serialization */ + NonlinearEquality2() {} + +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); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2", + boost::serialization::base_object(*this)); + } +}; + +} diff --git a/gtsam/slam/BetweenConstraint.h b/gtsam/slam/BetweenConstraint.h new file mode 100644 index 000000000..327fd465d --- /dev/null +++ b/gtsam/slam/BetweenConstraint.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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_; + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; + +} // \namespace gtsam diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h new file mode 100644 index 000000000..c116bcc8f --- /dev/null +++ b/gtsam/slam/BoundingConstraint.h @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * 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 VALUES& 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) { + if (isGreaterThan_) *H = D; + else *H = -1.0 * D; + } + + if (isGreaterThan_) + return Vector_(1, error); + else + return -1.0 * Vector_(1, error); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(threshold_); + ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); + } +}; + +/** + * 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 VALUES& 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) { + if (isGreaterThan_) *H1 = D1; + else *H1 = -1.0 * D1; + } + if (H2) { + if (isGreaterThan_) *H2 = D2; + else *H2 = -1.0 * D2; + } + + if (isGreaterThan_) + return Vector_(1, error); + else + return -1.0 * Vector_(1, error); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearConstraint2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(threshold_); + ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); + } +}; + +} // \namespace gtsam diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 3c6eba813..79a4e5773 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -14,6 +14,7 @@ check_PROGRAMS = headers += Simulated2DValues.h headers += Simulated2DPosePrior.h Simulated2DPointPrior.h headers += Simulated2DOdometry.h Simulated2DMeasurement.h +headers += simulated2DConstraints.h sources += simulated2D.cpp smallExample.cpp check_PROGRAMS += tests/testSimulated2D @@ -31,6 +32,9 @@ check_PROGRAMS += tests/testSimulated3D # Generic SLAM headers headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h +# Generic constraint headers +headers += BetweenConstraint.h BoundingConstraint.h + # 2D Pose SLAM sources += pose2SLAM.cpp dataset.cpp #sources += Pose2SLAMOptimizer.cpp diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h new file mode 100644 index 000000000..6dd05f14e --- /dev/null +++ b/gtsam/slam/simulated2DConstraints.h @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * 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 simulated2DConstraints.h + * @brief measurement functions and constraint definitions for simulated 2D robot + * @author Alex Cunningham + */ + +// \callgraph + +#pragma once + +#include + +#include +#include +#include +#include + +// \namespace + +namespace gtsam { + + namespace simulated2D { + + namespace equality_constraints { + + /** Typedefs for regular use */ + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; + + /** Equality between variables */ + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; + + } // \namespace equality_constraints + + namespace inequality_constraints { + + /** + * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) + * Demo implementation: should be made more general using BoundingConstraint + */ + template + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; + typedef boost::shared_ptr > shared_ptr; + + ScalarCoordConstraint1(const Key& key, double c, + bool isGreaterThan, double mu = 1000.0) : + Base(key, c, isGreaterThan, mu) { + } + + inline unsigned int index() const { return Idx; } + + /** extracts a single value from the point */ + virtual double value(const Point2& x, boost::optional H = + boost::none) const { + if (H) { + Matrix D = zeros(1, 2); + D(0, Idx) = 1.0; + *H = D; + } + return x.vector()(Idx); + } + }; + + /** typedefs for use with simulated2D systems */ + typedef ScalarCoordConstraint1 PoseXInequality; + typedef ScalarCoordConstraint1 PoseYInequality; + + double range(const Point2& a, const Point2& b) { return a.dist(b); } + + /** + * Binary inequality constraint forcing the range between points + * to be less than or equal to a bound + */ + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; + + MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0) + : Base(key1, key2, range_bound, false, mu) {} + + /** extracts a single scalar value with derivatives */ + virtual double value(const Point2& x1, const Point2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); + return x1.dist(x2); + } + }; + + typedef MaxDistanceConstraint PoseMaxDistConstraint; + + /** + * Binary inequality constraint forcing a minimum range + * NOTE: this is not a convex function! Be careful with initialization. + */ + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; + + MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0) + : Base(key1, key2, range_bound, true, mu) {} + + /** extracts a single scalar value with derivatives */ + virtual double value(const Point2& x1, const Point2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); + return x1.dist(x2); + } + }; + + typedef MinDistanceConstraint LandmarkAvoid; + + + } // \namespace inequality_constraints + + } // \namespace simulated2D +} // \namespace gtsam diff --git a/tests/Makefile.am b/tests/Makefile.am index acf78e5db..979686ce5 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -20,6 +20,8 @@ check_PROGRAMS += testNonlinearOptimizer check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph check_PROGRAMS += testTupleValues check_PROGRAMS += testNonlinearISAM +check_PROGRAMS += testBoundingConstraint +check_PROGRAMS += testNonlinearEqualityConstraint # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp new file mode 100644 index 000000000..711f103ed --- /dev/null +++ b/tests/testBoundingConstraint.cpp @@ -0,0 +1,287 @@ +/* ---------------------------------------------------------------------------- + + * 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); + Ordering ordering; + ordering += key; + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1, ordering); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1, ordering); + EXPECT(!actual1); + EXPECT(!actual2); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_linearization_active) { + Point2 pt2(-2.0, -3.0); + simulated2D::Values config2; + config2.insert(key, pt2); + Ordering ordering; + ordering += key; + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2, ordering); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2, ordering); + JacobianFactor expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); + JacobianFactor expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); + EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); + EXPECT(assert_equal((const GaussianFactor&)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); + 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); + 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); + Ordering ordering; ordering += key1, key2; + EXPECT(!rangeBound.active(config1)); + EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(!rangeBound.linearize(config1, ordering)); + 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, ordering)); + 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); + + simulated2D::Values expected; + expected.insert(x1, pt1); + expected.insert(x2, pt2_goal); + + // FAILS: segfaults on optimization +// Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initial_state); +// 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); + + // FAILS: segfaults on optimization +// 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/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp new file mode 100644 index 000000000..ee14349f1 --- /dev/null +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -0,0 +1,369 @@ +/* ---------------------------------------------------------------------------- + + * 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; + Ordering ordering; + ordering += key; + eq2D::UnaryEqualityConstraint constraint(pt, key, mu); + + simulated2D::Values config1; + config1.insert(key, pt); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); + GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[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, ordering); + GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[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; + Ordering ordering; + ordering += key1, key2; + 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, ordering); + GaussianFactor::shared_ptr expected1( + new JacobianFactor(ordering[key1], -eye(2,2), ordering[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, ordering); + GaussianFactor::shared_ptr expected2( + new JacobianFactor(ordering[key1], -eye(2,2), ordering[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); } +/* ************************************************************************* */ + +