Added nonlinear constraints to gtsam library
							parent
							
								
									79c09708e8
								
							
						
					
					
						commit
						9160775d2a
					
				|  | @ -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
 | ||||
|  |  | |||
|  | @ -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 <map> | ||||
| #include <boost/function.hpp> | ||||
| #include <gtsam/inference/IndexFactor.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| 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 VALUES> | ||||
| class NonlinearConstraint : public NonlinearFactor<VALUES> { | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearConstraint<VALUES> This; | ||||
| 	typedef NonlinearFactor<VALUES> 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<VALUES>& f, double tol=1e-9) const { | ||||
| 		const This* p = dynamic_cast<const This*> (&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<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const=0; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
| 	/** Serialization function */ | ||||
| 	friend class boost::serialization::access; | ||||
| 	template<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearFactor", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(mu_); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /**
 | ||||
|  * A unary constraint that defaults to an equality constraint | ||||
|  */ | ||||
| template <class VALUES, class KEY> | ||||
| class NonlinearConstraint1 : public NonlinearConstraint<VALUES> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY::Value X; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearConstraint1<VALUES,KEY> This; | ||||
| 	typedef NonlinearConstraint<VALUES> 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<VALUES>& f, double tol = 1e-9) const { | ||||
| 		const This* p = dynamic_cast<const This*> (&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<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const { | ||||
| 		if (!active(x)) { | ||||
| 			boost::shared_ptr<JacobianFactor> 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<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(key_); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * Unary Equality constraint - simply forces the value of active() to true | ||||
|  */ | ||||
| template <class VALUES, class KEY> | ||||
| class NonlinearEqualityConstraint1 : public NonlinearConstraint1<VALUES, KEY> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY::Value X; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearEqualityConstraint1<VALUES,KEY> This; | ||||
| 	typedef NonlinearConstraint1<VALUES,KEY> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint1", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 	} | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * A binary constraint with arbitrary cost and jacobian functions | ||||
|  */ | ||||
| template <class VALUES, class KEY1, class KEY2> | ||||
| class NonlinearConstraint2 : public NonlinearConstraint<VALUES> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY1::Value X1; | ||||
| 	typedef typename KEY2::Value X2; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearConstraint2<VALUES,KEY1,KEY2> This; | ||||
| 	typedef NonlinearConstraint<VALUES> 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<VALUES>& f, double tol = 1e-9) const { | ||||
| 		const This* p = dynamic_cast<const This*> (&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<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const { | ||||
| 		if (!active(c)) { | ||||
| 			boost::shared_ptr<JacobianFactor> 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<Matrix&> H1 = boost::none, | ||||
| 			boost::optional<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(key1_); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(key2_); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * Binary Equality constraint - simply forces the value of active() to true | ||||
|  */ | ||||
| template <class VALUES, class KEY1, class KEY2> | ||||
| class NonlinearEqualityConstraint2 : public NonlinearConstraint2<VALUES, KEY1, KEY2> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY1::Value X1; | ||||
| 	typedef typename KEY2::Value X2; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearEqualityConstraint2<VALUES,KEY1,KEY2> This; | ||||
| 	typedef NonlinearConstraint2<VALUES,KEY1,KEY2> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint2", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * A ternary constraint | ||||
|  */ | ||||
| template <class VALUES, class KEY1, class KEY2, class KEY3> | ||||
| class NonlinearConstraint3 : public NonlinearConstraint<VALUES> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY1::Value X1; | ||||
| 	typedef typename KEY2::Value X2; | ||||
| 	typedef typename KEY3::Value X3; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> This; | ||||
| 	typedef NonlinearConstraint<VALUES> 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<VALUES>& f, double tol = 1e-9) const { | ||||
| 		const This* p = dynamic_cast<const This*> (&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<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const { | ||||
| 		if (!active(c)) { | ||||
| 			boost::shared_ptr<JacobianFactor> 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<Matrix&> H1 = boost::none, | ||||
| 			boost::optional<Matrix&> H2 = boost::none, | ||||
| 			boost::optional<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint", | ||||
| 				boost::serialization::base_object<Base>(*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 VALUES, class KEY1, class KEY2, class KEY3> | ||||
| class NonlinearEqualityConstraint3 : public NonlinearConstraint3<VALUES, KEY1, KEY2, KEY3> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY1::Value X1; | ||||
| 	typedef typename KEY2::Value X2; | ||||
| 	typedef typename KEY3::Value X3; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearEqualityConstraint3<VALUES,KEY1,KEY2,KEY3> This; | ||||
| 	typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint3", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /**
 | ||||
|  * Simple unary equality constraint - fixes a value for a variable | ||||
|  */ | ||||
| template<class VALUES, class KEY> | ||||
| class NonlinearEquality1 : public NonlinearEqualityConstraint1<VALUES, KEY> { | ||||
| 
 | ||||
| public: | ||||
| 	typedef typename KEY::Value X; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearEqualityConstraint1<VALUES, KEY> Base; | ||||
| 
 | ||||
| 	/** default constructor to allow for serialization */ | ||||
| 	NonlinearEquality1() {} | ||||
| 
 | ||||
| 	X value_; /// fixed value for variable
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 	typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > 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<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearEqualityConstraint1", | ||||
| 				boost::serialization::base_object<Base>(*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 VALUES, class KEY> | ||||
| class NonlinearEquality2 : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> { | ||||
| public: | ||||
| 	typedef typename KEY::Value X; | ||||
| 
 | ||||
| protected: | ||||
| 	typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base; | ||||
| 
 | ||||
| 	/** default constructor to allow for serialization */ | ||||
| 	NonlinearEquality2() {} | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 	typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > 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<Matrix&> H1 = boost::none, | ||||
| 			boost::optional<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| } | ||||
|  | @ -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 <gtsam/nonlinear/NonlinearConstraint.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Binary between constraint - forces between to a given value | ||||
| 	 * This constraint requires the underlying type to a Lie type | ||||
| 	 */ | ||||
| 	template<class VALUES, class KEY> | ||||
| 	class BetweenConstraint : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> { | ||||
| 	public: | ||||
| 		typedef typename KEY::Value X; | ||||
| 
 | ||||
| 	protected: | ||||
| 		typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base; | ||||
| 
 | ||||
| 		X measured_; /// fixed between value
 | ||||
| 
 | ||||
| 	public: | ||||
| 
 | ||||
| 		typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > 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<Matrix&> H1 = boost::none, | ||||
| 				boost::optional<Matrix&> 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<class ARCHIVE> | ||||
| 		void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 			ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2", | ||||
| 					boost::serialization::base_object<Base>(*this)); | ||||
| 			ar & BOOST_SERIALIZATION_NVP(measured_); | ||||
| 		} | ||||
| 	}; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  | @ -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 <gtsam/base/Lie.h> | ||||
| #include <gtsam/nonlinear/NonlinearConstraint.h> | ||||
| 
 | ||||
| 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<class VALUES, class KEY> | ||||
| struct BoundingConstraint1: public NonlinearConstraint1<VALUES, KEY> { | ||||
| 	typedef typename KEY::Value X; | ||||
| 	typedef NonlinearConstraint1<VALUES, KEY> Base; | ||||
| 	typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > 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<Matrix&> 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<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint1", | ||||
| 				boost::serialization::base_object<Base>(*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<class VALUES, class KEY1, class KEY2> | ||||
| struct BoundingConstraint2: public NonlinearConstraint2<VALUES, KEY1, KEY2> { | ||||
| 	typedef typename KEY1::Value X1; | ||||
| 	typedef typename KEY2::Value X2; | ||||
| 
 | ||||
| 	typedef NonlinearConstraint2<VALUES, KEY1, KEY2> Base; | ||||
| 	typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > 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<Matrix&> H1 = boost::none, | ||||
| 			boost::optional<Matrix&> 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<Matrix&> H1 = boost::none, | ||||
| 			boost::optional<Matrix&> 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<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearConstraint2", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(threshold_); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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 <gtsam/base/numericalDerivative.h> | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearConstraint.h> | ||||
| #include <gtsam/slam/BetweenConstraint.h> | ||||
| #include <gtsam/slam/BoundingConstraint.h> | ||||
| #include <gtsam/slam/simulated2D.h> | ||||
| 
 | ||||
| // \namespace
 | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 	namespace simulated2D { | ||||
| 
 | ||||
| 		namespace equality_constraints { | ||||
| 
 | ||||
| 			/** Typedefs for regular use */ | ||||
| 			typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint; | ||||
| 			typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint; | ||||
| 			typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint; | ||||
| 
 | ||||
| 			/** Equality between variables */ | ||||
| 			typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint; | ||||
| 			typedef NonlinearEquality2<Values, PointKey> 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<class Cfg, class Key, unsigned int Idx> | ||||
| 			struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key> { | ||||
| 				typedef BoundingConstraint1<Cfg, Key> Base; | ||||
| 				typedef boost::shared_ptr<ScalarCoordConstraint1<Cfg, Key, Idx> > 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<Matrix&> 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<Values, PoseKey, 0> PoseXInequality; | ||||
| 			typedef ScalarCoordConstraint1<Values, PoseKey, 1> 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<class Cfg, class Key> | ||||
| 			struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Key> { | ||||
| 				typedef BoundingConstraint2<Cfg, Key, Key> 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<Matrix&> H1 = boost::none, | ||||
| 						boost::optional<Matrix&> 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<Values, PoseKey> PoseMaxDistConstraint; | ||||
| 
 | ||||
| 			/**
 | ||||
| 			 * Binary inequality constraint forcing a minimum range | ||||
| 			 * NOTE: this is not a convex function!  Be careful with initialization. | ||||
| 			 */ | ||||
| 			template<class Cfg, class XKey, class PKey> | ||||
| 			struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, PKey> { | ||||
| 				typedef BoundingConstraint2<Cfg, XKey, PKey> 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<Matrix&> H1 = boost::none, | ||||
| 						boost::optional<Matrix&> 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<Values, PoseKey, PointKey> LandmarkAvoid; | ||||
| 
 | ||||
| 
 | ||||
| 		} // \namespace inequality_constraints
 | ||||
| 
 | ||||
| 	} // \namespace simulated2D
 | ||||
| } // \namespace gtsam
 | ||||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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 <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/slam/simulated2DConstraints.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> | ||||
| #include <gtsam/nonlinear/NonlinearOptimizer-inl.h> | ||||
| 
 | ||||
| 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<simulated2D::Values> Graph; | ||||
| typedef boost::shared_ptr<Graph> shared_graph; | ||||
| typedef boost::shared_ptr<simulated2D::Values> shared_values; | ||||
| typedef NonlinearOptimizer<Graph, simulated2D::Values> 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); } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  | @ -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 <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/slam/simulated2DConstraints.h> | ||||
| #include <gtsam/slam/visualSLAM.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> | ||||
| #include <gtsam/nonlinear/NonlinearOptimizer-inl.h> | ||||
| 
 | ||||
| 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<simulated2D::Values> Graph; | ||||
| typedef boost::shared_ptr<Graph> shared_graph; | ||||
| typedef boost::shared_ptr<simulated2D::Values> shared_values; | ||||
| typedef NonlinearOptimizer<Graph, simulated2D::Values> 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<Cal3_S2> shK(new Cal3_S2(K)); | ||||
| 
 | ||||
| // typedefs for visual SLAM example
 | ||||
| typedef visualSLAM::Values VValues; | ||||
| typedef boost::shared_ptr<VValues> shared_vconfig; | ||||
| typedef visualSLAM::Graph VGraph; | ||||
| typedef NonlinearOptimizer<VGraph,VValues> VOptimizer; | ||||
| 
 | ||||
| // factors for visual slam
 | ||||
| typedef NonlinearEquality2<VValues, visualSLAM::PointKey> 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); } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| 
 | ||||
		Loading…
	
		Reference in New Issue