case change: SharedGaussian and SharedDiagonal are now classes with their own header file. Needed for MATLAB TORO hail Mary
							parent
							
								
									490791cd48
								
							
						
					
					
						commit
						351cdd18c2
					
				|  | @ -33,7 +33,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		/** Constructor */ | ||||
| 		BetweenFactor(const Key& key1, const Key& key2, const T& measured, | ||||
| 				const sharedGaussian& model) : | ||||
| 				const SharedGaussian& model) : | ||||
| 			Base(model, key1, key2), measured_(measured) { | ||||
| 		} | ||||
| 
 | ||||
|  |  | |||
|  | @ -323,7 +323,7 @@ GaussianFactor::eliminate(const Symbol& key) const | |||
| 	Matrix Ab = matrix_augmented(ordering,false); | ||||
| 
 | ||||
| 	// Use in-place QR on dense Ab appropriate to NoiseModel
 | ||||
| 	sharedDiagonal noiseModel = model_->QR(Ab); | ||||
| 	SharedDiagonal noiseModel = model_->QR(Ab); | ||||
| 
 | ||||
| 	// get dimensions of the eliminated variable
 | ||||
| 	// TODO: this is another map find that should be avoided !
 | ||||
|  |  | |||
|  | @ -18,7 +18,7 @@ | |||
| #include "Factor.h" | ||||
| #include "Matrix.h" | ||||
| #include "VectorConfig.h" | ||||
| #include "NoiseModel.h" | ||||
| #include "SharedDiagonal.h" | ||||
| #include "SymbolMap.h" | ||||
| 
 | ||||
| namespace gtsam { | ||||
|  | @ -40,7 +40,7 @@ public: | |||
| 
 | ||||
| protected: | ||||
| 
 | ||||
| 	sharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
 | ||||
| 	SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
 | ||||
| 	SymbolMap<Matrix> As_; // linear matrices
 | ||||
| 	Vector b_; // right-hand-side
 | ||||
| 
 | ||||
|  | @ -57,7 +57,7 @@ public: | |||
| 
 | ||||
| 	/** Construct unary factor */ | ||||
| 	GaussianFactor(const Symbol& key1, const Matrix& A1, | ||||
| 			const Vector& b, const sharedDiagonal& model) : | ||||
| 			const Vector& b, const SharedDiagonal& model) : | ||||
| 		b_(b), model_(model) { | ||||
| 		As_.insert(make_pair(key1, A1)); | ||||
| 	} | ||||
|  | @ -65,7 +65,7 @@ public: | |||
| 	/** Construct binary factor */ | ||||
| 	GaussianFactor(const Symbol& key1, const Matrix& A1, | ||||
| 			const Symbol& key2, const Matrix& A2, | ||||
| 			const Vector& b, const sharedDiagonal& model) : | ||||
| 			const Vector& b, const SharedDiagonal& model) : | ||||
| 		b_(b), model_(model)  { | ||||
| 		As_.insert(make_pair(key1, A1)); | ||||
| 		As_.insert(make_pair(key2, A2)); | ||||
|  | @ -75,7 +75,7 @@ public: | |||
| 	GaussianFactor(const Symbol& key1, const Matrix& A1, | ||||
| 			const Symbol& key2, const Matrix& A2, | ||||
| 			const Symbol& key3, const Matrix& A3, | ||||
| 			const Vector& b, const sharedDiagonal& model) : | ||||
| 			const Vector& b, const SharedDiagonal& model) : | ||||
| 		b_(b), model_(model)  { | ||||
| 		As_.insert(make_pair(key1, A1)); | ||||
| 		As_.insert(make_pair(key2, A2)); | ||||
|  | @ -84,7 +84,7 @@ public: | |||
| 
 | ||||
| 	/** Construct an n-ary factor */ | ||||
| 	GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms, | ||||
| 	    const Vector &b, const sharedDiagonal& model) : | ||||
| 	    const Vector &b, const SharedDiagonal& model) : | ||||
| 	    b_(b), model_(model)  { | ||||
| 	  for(unsigned int i=0; i<terms.size(); i++) | ||||
| 	    As_.insert(terms[i]); | ||||
|  | @ -127,7 +127,7 @@ public: | |||
| 	const Vector& get_sigmas() const {	return model_->sigmas();	} | ||||
| 
 | ||||
| 	/** get a copy of model */ | ||||
| 	const sharedDiagonal& get_model() const { return model_;  } | ||||
| 	const SharedDiagonal& get_model() const { return model_;  } | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * get a copy of the A matrix from a specific node | ||||
|  |  | |||
|  | @ -181,7 +181,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const { | |||
| 	FOREACH_PAIR(key,dim,vs) { | ||||
| 		Matrix A = eye(dim); | ||||
| 		Vector b = zero(dim); | ||||
| 		sharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); | ||||
| 		SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); | ||||
| 		sharedFactor prior(new GaussianFactor(key,A,b, model)); | ||||
| 		result.push_back(prior); | ||||
| 	} | ||||
|  |  | |||
|  | @ -48,14 +48,14 @@ namespace gtsam { | |||
| 
 | ||||
|   	/** Add a unary factor */ | ||||
|     inline void add(const Symbol& key1, const Matrix& A1, | ||||
|   			const Vector& b, const sharedDiagonal& model) { | ||||
|   			const Vector& b, const SharedDiagonal& model) { | ||||
|     	push_back(sharedFactor(new GaussianFactor(key1,A1,b,model))); | ||||
|   	} | ||||
| 
 | ||||
|   	/** Add a binary factor */ | ||||
|     inline void add(const Symbol& key1, const Matrix& A1, | ||||
|   			const Symbol& key2, const Matrix& A2, | ||||
|   			const Vector& b, const sharedDiagonal& model) { | ||||
|   			const Vector& b, const SharedDiagonal& model) { | ||||
|     	push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model))); | ||||
|   	} | ||||
| 
 | ||||
|  | @ -63,13 +63,13 @@ namespace gtsam { | |||
|     inline void add(const Symbol& key1, const Matrix& A1, | ||||
|   			const Symbol& key2, const Matrix& A2, | ||||
|   			const Symbol& key3, const Matrix& A3, | ||||
|   			const Vector& b, const sharedDiagonal& model) { | ||||
|   			const Vector& b, const SharedDiagonal& model) { | ||||
|     	push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model))); | ||||
|   	} | ||||
| 
 | ||||
|   	/** Add an n-ary factor */ | ||||
|     inline void add(const std::vector<std::pair<Symbol, Matrix> > &terms, | ||||
|   	    const Vector &b, const sharedDiagonal& model) { | ||||
|   	    const Vector &b, const SharedDiagonal& model) { | ||||
|     	push_back(sharedFactor(new GaussianFactor(terms,b,model))); | ||||
|   	} | ||||
| 
 | ||||
|  |  | |||
|  | @ -88,7 +88,7 @@ testBinaryBayesNet_SOURCES    = testBinaryBayesNet.cpp | |||
| testBinaryBayesNet_LDADD      = libgtsam.la  | ||||
| 
 | ||||
| # Gaussian inference 
 | ||||
| headers += GaussianFactorSet.h  | ||||
| headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h | ||||
| sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp | ||||
| check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel | ||||
| testVectorConfig_SOURCES        = testVectorConfig.cpp | ||||
|  |  | |||
|  | @ -24,6 +24,7 @@ | |||
| #include <boost/random/variate_generator.hpp> | ||||
| 
 | ||||
| #include "NoiseModel.h" | ||||
| #include "SharedDiagonal.h" | ||||
| 
 | ||||
| namespace ublas = boost::numeric::ublas; | ||||
| typedef ublas::matrix_column<Matrix> column; | ||||
|  | @ -121,7 +122,7 @@ namespace gtsam { | |||
| 		} | ||||
| 
 | ||||
| 		// General QR, see also special version in Constrained
 | ||||
| 		sharedDiagonal Gaussian::QR(Matrix& Ab) const { | ||||
| 		SharedDiagonal Gaussian::QR(Matrix& Ab) const { | ||||
| 			// get size(A) and maxRank
 | ||||
| 			// TODO: really no rank problems ?
 | ||||
| 			size_t m = Ab.size1(), n = Ab.size2()-1; | ||||
|  | @ -207,7 +208,7 @@ namespace gtsam { | |||
| 		// Special version of QR for Constrained calls slower but smarter code
 | ||||
| 		// that deals with possibly zero sigmas
 | ||||
| 		// It is Gram-Schmidt orthogonalization rather than Householder
 | ||||
| 		sharedDiagonal Diagonal::QR(Matrix& Ab) const { | ||||
| 		SharedDiagonal Diagonal::QR(Matrix& Ab) const { | ||||
| 			// get size(A) and maxRank
 | ||||
| 			size_t m = Ab.size1(), n = Ab.size2()-1; | ||||
| 			size_t maxRank = min(m,n); | ||||
|  |  | |||
|  | @ -15,7 +15,7 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 	class sharedDiagonal; // forward declare, defined at end
 | ||||
| 	class SharedDiagonal; // forward declare, defined at end
 | ||||
| 
 | ||||
| 	namespace noiseModel { | ||||
| 
 | ||||
|  | @ -146,7 +146,7 @@ namespace gtsam { | |||
|      * @param Ab is the m*(n+1) augmented system matrix [A b] | ||||
|      * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! | ||||
|      */ | ||||
|     virtual sharedDiagonal QR(Matrix& Ab) const; | ||||
|     virtual SharedDiagonal QR(Matrix& Ab) const; | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Return R itself, but note that Whiten(H) is cheaper than R*H | ||||
|  | @ -210,7 +210,7 @@ namespace gtsam { | |||
|     /**
 | ||||
|      * Apply QR factorization to the system [A b], taking into account constraints | ||||
|      */ | ||||
| 		virtual sharedDiagonal QR(Matrix& Ab) const; | ||||
| 		virtual SharedDiagonal QR(Matrix& Ab) const; | ||||
| 
 | ||||
| 		/**
 | ||||
|      * Return standard deviations (sqrt of diagonal) | ||||
|  | @ -373,42 +373,5 @@ namespace gtsam { | |||
| 
 | ||||
| 	} // namespace noiseModel
 | ||||
| 
 | ||||
| 	using namespace noiseModel; | ||||
| 
 | ||||
| 	// TODO: very ugly, just keep shared pointers and use Sigma/Sigmas everywhere
 | ||||
| 
 | ||||
| 	// A useful convenience class to refer to a shared Gaussian model
 | ||||
| 	// Define GTSAM_MAGIC_GAUSSIAN to desired dimension to have access to slightly
 | ||||
| 	// dangerous and non-shared (inefficient, wasteful) noise models. Only in tests!
 | ||||
| 	struct sharedGaussian : public Gaussian::shared_ptr { | ||||
| 		sharedGaussian() {} | ||||
| 		sharedGaussian(const    Gaussian::shared_ptr& p):Gaussian::shared_ptr(p) {} | ||||
| 		sharedGaussian(const    Diagonal::shared_ptr& p):Gaussian::shared_ptr(p) {} | ||||
| 		sharedGaussian(const Constrained::shared_ptr& p):Gaussian::shared_ptr(p) {} | ||||
| 		sharedGaussian(const   Isotropic::shared_ptr& p):Gaussian::shared_ptr(p) {} | ||||
| 		sharedGaussian(const        Unit::shared_ptr& p):Gaussian::shared_ptr(p) {} | ||||
| #ifdef GTSAM_MAGIC_GAUSSIAN | ||||
| 		sharedGaussian(const Matrix& covariance):Gaussian::shared_ptr(Gaussian::Covariance(covariance)) {} | ||||
| 		sharedGaussian(const Vector& sigmas):Gaussian::shared_ptr(Diagonal::Sigmas(sigmas)) {} | ||||
| 		sharedGaussian(const double& s):Gaussian::shared_ptr(Isotropic::Sigma(GTSAM_MAGIC_GAUSSIAN,s)) {} | ||||
| #endif | ||||
| 		}; | ||||
| 
 | ||||
| 	// A useful convenience class to refer to a shared Diagonal model
 | ||||
| 	// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
 | ||||
| 	// that call Sigmas and Sigma, respectively.
 | ||||
| 	struct sharedDiagonal : public Diagonal::shared_ptr { | ||||
| 		sharedDiagonal() {} | ||||
| 		sharedDiagonal(const    Diagonal::shared_ptr& p):Diagonal::shared_ptr(p) {} | ||||
| 		sharedDiagonal(const Constrained::shared_ptr& p):Diagonal::shared_ptr(p) {} | ||||
| 		sharedDiagonal(const   Isotropic::shared_ptr& p):Diagonal::shared_ptr(p) {} | ||||
| 		sharedDiagonal(const        Unit::shared_ptr& p):Diagonal::shared_ptr(p) {} | ||||
| 		sharedDiagonal(const Vector& sigmas):Diagonal::shared_ptr(Diagonal::Sigmas(sigmas)) {} | ||||
| 		}; | ||||
| 
 | ||||
| 	// TODO: make these the ones really used in unit tests
 | ||||
| 	inline sharedDiagonal sharedSigmas(const Vector& sigmas) { return Diagonal::Sigmas(sigmas);} | ||||
| 	inline sharedDiagonal sharedSigma(int dim, double sigma) { return Isotropic::Sigma(dim,sigma);} | ||||
| 
 | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -122,12 +122,12 @@ NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const Vect | |||
| 
 | ||||
| 	// construct probabilistic factor
 | ||||
| 	Matrix A1 = vector_scale(lambda, grad); | ||||
| 	sharedDiagonal probModel = sharedSigma(this->p_,1.0); | ||||
| 	SharedDiagonal probModel = sharedSigma(this->p_,1.0); | ||||
| 	GaussianFactor::shared_ptr factor(new | ||||
| 			GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); | ||||
| 
 | ||||
| 	// construct the constraint
 | ||||
| 	sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); | ||||
| 	SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); | ||||
| 	GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel)); | ||||
| 
 | ||||
| 	return std::make_pair(factor, constraint); | ||||
|  | @ -222,12 +222,12 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst | |||
| 	// construct probabilistic factor
 | ||||
| 	Matrix A1 = vector_scale(lambda, grad1); | ||||
| 	Matrix A2 = vector_scale(lambda, grad2); | ||||
| 	sharedDiagonal probModel = sharedSigma(this->p_,1.0); | ||||
| 	SharedDiagonal probModel = sharedSigma(this->p_,1.0); | ||||
| 	GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2, | ||||
| 			this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); | ||||
| 
 | ||||
| 	// construct the constraint
 | ||||
| 	sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); | ||||
| 	SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); | ||||
| 	GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, | ||||
| 			key2_, grad2, -1.0 * g, constraintModel)); | ||||
| 
 | ||||
|  |  | |||
|  | @ -83,7 +83,7 @@ namespace gtsam { | |||
| 			Matrix A; | ||||
| 			Vector b = - evaluateError(xj, A); | ||||
| 			// TODO pass unwhitened + noise model to Gaussian factor
 | ||||
| 			sharedDiagonal model = noiseModel::Constrained::All(b.size()); | ||||
| 			SharedDiagonal model = noiseModel::Constrained::All(b.size()); | ||||
| 			return	GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, model)); | ||||
| 		} | ||||
| 
 | ||||
|  |  | |||
|  | @ -18,7 +18,7 @@ | |||
| #include "Factor.h" | ||||
| #include "Vector.h" | ||||
| #include "Matrix.h" | ||||
| #include "NoiseModel.h" | ||||
| #include "SharedGaussian.h" | ||||
| #include "GaussianFactor.h" | ||||
| 
 | ||||
| #define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \ | ||||
|  | @ -43,7 +43,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		typedef NonlinearFactor<Config> This; | ||||
| 
 | ||||
| 		sharedGaussian noiseModel_; /** Noise model */ | ||||
| 		SharedGaussian noiseModel_; /** Noise model */ | ||||
| 		std::list<Symbol> keys_; /** cached keys */ | ||||
| 
 | ||||
| 	public: | ||||
|  | @ -56,7 +56,7 @@ namespace gtsam { | |||
| 		 *  Constructor | ||||
| 		 *  @param noiseModel shared pointer to a noise model | ||||
| 		 */ | ||||
| 		NonlinearFactor(const sharedGaussian& noiseModel) : | ||||
| 		NonlinearFactor(const SharedGaussian& noiseModel) : | ||||
| 			noiseModel_(noiseModel) { | ||||
| 		} | ||||
| 
 | ||||
|  | @ -148,7 +148,7 @@ namespace gtsam { | |||
| 		 *  @param z measurement | ||||
| 		 *  @param key by which to look up X value in Config | ||||
| 		 */ | ||||
| 		NonlinearFactor1(const sharedGaussian& noiseModel, | ||||
| 		NonlinearFactor1(const SharedGaussian& noiseModel, | ||||
| 				const Key& key1) : | ||||
| 			Base(noiseModel), key_(key1) { | ||||
| 			this->keys_.push_back(key_); | ||||
|  | @ -242,7 +242,7 @@ namespace gtsam { | |||
| 		 * @param j1 key of the first variable | ||||
| 		 * @param j2 key of the second variable | ||||
| 		 */ | ||||
| 		NonlinearFactor2(const sharedGaussian& noiseModel, Key1 j1, | ||||
| 		NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1, | ||||
| 				Key2 j2) : | ||||
| 			Base(noiseModel), key1_(j1), key2_(j2) { | ||||
| 			this->keys_.push_back(key1_); | ||||
|  |  | |||
|  | @ -31,7 +31,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		/** Constructor */ | ||||
| 		PriorFactor(const Key& key, const T& prior, | ||||
| 				const sharedGaussian& model) : | ||||
| 				const SharedGaussian& model) : | ||||
| 			Base(model, key), prior_(prior) { | ||||
| 		} | ||||
| 
 | ||||
|  |  | |||
|  | @ -0,0 +1,45 @@ | |||
| /*
 | ||||
|  * SharedDiagonal.h | ||||
|  * @brief Class that wraps a shared noise model with diagonal covariance | ||||
|  * @Author: Frank Dellaert | ||||
|  * Created on: Jan 22, 2010 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include "NoiseModel.h" | ||||
| 
 | ||||
| namespace gtsam { // note, deliberately not in noiseModel namespace
 | ||||
| 
 | ||||
| 	// A useful convenience class to refer to a shared Diagonal model
 | ||||
| 	// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
 | ||||
| 	// that call Sigmas and Sigma, respectively.
 | ||||
| 	struct SharedDiagonal: public noiseModel::Diagonal::shared_ptr { | ||||
| 		SharedDiagonal() { | ||||
| 		} | ||||
| 		SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) : | ||||
| 			noiseModel::Diagonal::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedDiagonal(const noiseModel::Constrained::shared_ptr& p) : | ||||
| 			noiseModel::Diagonal::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedDiagonal(const noiseModel::Isotropic::shared_ptr& p) : | ||||
| 			noiseModel::Diagonal::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedDiagonal(const noiseModel::Unit::shared_ptr& p) : | ||||
| 			noiseModel::Diagonal::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedDiagonal(const Vector& sigmas) : | ||||
| 			noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { | ||||
| 		} | ||||
| 	}; | ||||
| 
 | ||||
| 	// TODO: make these the ones really used in unit tests
 | ||||
| 	inline SharedDiagonal sharedSigmas(const Vector& sigmas) { | ||||
| 		return noiseModel::Diagonal::Sigmas(sigmas); | ||||
| 	} | ||||
| 	inline SharedDiagonal sharedSigma(int dim, double sigma) { | ||||
| 		return noiseModel::Isotropic::Sigma(dim, sigma); | ||||
| 	} | ||||
| 
 | ||||
| } | ||||
|  | @ -0,0 +1,59 @@ | |||
| /*
 | ||||
|  * SharedGaussian.h | ||||
|  * @brief Class that wraps a shared noise model with diagonal covariance | ||||
|  * @Author: Frank Dellaert | ||||
|  * Created on: Jan 22, 2010 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include "NoiseModel.h" | ||||
| 
 | ||||
| namespace gtsam { // note, deliberately not in noiseModel namespace
 | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * A useful convenience class to refer to a shared Gaussian model | ||||
| 	 * Also needed to make noise models in matlab | ||||
| 	 */ | ||||
| 	struct SharedGaussian: public noiseModel::Gaussian::shared_ptr { | ||||
| 		SharedGaussian() { | ||||
| 		} | ||||
| 		// TODO: better way ?
 | ||||
| 		SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) : | ||||
| 			noiseModel::Gaussian::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) : | ||||
| 			noiseModel::Gaussian::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedGaussian(const noiseModel::Constrained::shared_ptr& p) : | ||||
| 			noiseModel::Gaussian::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) : | ||||
| 			noiseModel::Gaussian::shared_ptr(p) { | ||||
| 		} | ||||
| 		SharedGaussian(const noiseModel::Unit::shared_ptr& p) : | ||||
| 			noiseModel::Gaussian::shared_ptr(p) { | ||||
| 		} | ||||
| 
 | ||||
| 		// Define GTSAM_MAGIC_GAUSSIAN to have access to slightly
 | ||||
| 		// dangerous and non-shared (inefficient, wasteful) noise models.
 | ||||
| 		// Intended to be used only in tests (if you must) and the MATLAB wrapper
 | ||||
| #ifdef GTSAM_MAGIC_GAUSSIAN | ||||
| 		SharedGaussian(const Matrix& covariance) : | ||||
| 			noiseModel::Gaussian::shared_ptr(noiseModel::Gaussian::Covariance(covariance)) { | ||||
| 		} | ||||
| 		SharedGaussian(const Vector& sigmas) : | ||||
| 			noiseModel::Gaussian::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { | ||||
| 		} | ||||
| #endif | ||||
| // Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians
 | ||||
| // Not intended for human use, only for backwards compatibility of old unit tests
 | ||||
| #ifdef GTSAM_DANGEROUS_GAUSSIAN | ||||
| 		SharedGaussian(const double& s) : | ||||
| 			noiseModel::Gaussian::shared_ptr(noiseModel::Isotropic::Sigma( | ||||
| 					GTSAM_DANGEROUS_GAUSSIAN, s)) { | ||||
| 		} | ||||
| #endif | ||||
| 	}; | ||||
| 
 | ||||
| } | ||||
|  | @ -49,7 +49,7 @@ namespace simulated3D { | |||
| 		Vector z_; | ||||
| 
 | ||||
| 		Point2Prior3D(const Vector& z, | ||||
| 					const sharedGaussian& model, const PoseKey& j) : | ||||
| 					const SharedGaussian& model, const PoseKey& j) : | ||||
| 				NonlinearFactor1<VectorConfig, PoseKey, Vector> (model, j), z_(z) { | ||||
| 			} | ||||
| 
 | ||||
|  | @ -66,7 +66,7 @@ namespace simulated3D { | |||
| 		Vector z_; | ||||
| 
 | ||||
| 		Simulated3DMeasurement(const Vector& z, | ||||
| 					const sharedGaussian& model, PoseKey& j1, PointKey j2) : | ||||
| 					const SharedGaussian& model, PoseKey& j1, PointKey j2) : | ||||
| 				z_(z), | ||||
| 						NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, Vector> ( | ||||
| 								model, j1, j2) { | ||||
|  |  | |||
|  | @ -0,0 +1,60 @@ | |||
| // These are currently broken
 | ||||
| // Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class
 | ||||
| // We also have to solve the shared pointer mess to avoid duplicate methods
 | ||||
| 
 | ||||
| class Point2Prior { | ||||
|   Point2Prior(Vector mu, double sigma, string key); | ||||
|   Vector error_vector(const VectorConfig& c) const; | ||||
|   GaussianFactor* linearize(const VectorConfig& c) const; | ||||
|   double sigma(); | ||||
|   Vector measurement(); | ||||
|   double error(const VectorConfig& c) const; | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Simulated2DOdometry { | ||||
|   Simulated2DOdometry(Vector odo, double sigma, string key, string key2); | ||||
|   Vector error_vector(const VectorConfig& c) const; | ||||
|   GaussianFactor* linearize(const VectorConfig& c) const; | ||||
|   double sigma(); | ||||
|   Vector measurement(); | ||||
|   double error(const VectorConfig& c) const; | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Simulated2DMeasurement { | ||||
|   Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); | ||||
|   Vector error_vector(const VectorConfig& c) const; | ||||
|   GaussianFactor* linearize(const VectorConfig& c) const; | ||||
|   double sigma(); | ||||
|   Vector measurement(); | ||||
|   double error(const VectorConfig& c) const; | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Pose2Config{ | ||||
| 	Pose2Config(); | ||||
| 	Pose2 get(string key) const; | ||||
| 	void insert(string name, const Pose2& val); | ||||
|     void print(string s) const; | ||||
|     void clear(); | ||||
|     int size(); | ||||
| }; | ||||
| 
 | ||||
| class Pose2Factor { | ||||
| 	Pose2Factor(string key1, string key2, | ||||
| 			const Pose2& measured, Matrix measurement_covariance); | ||||
| 	void print(string name) const; | ||||
| 	double error(const Pose2Config& c) const; | ||||
| 	size_t size() const; | ||||
| 	GaussianFactor* linearize(const Pose2Config& config) const; | ||||
| }; | ||||
| 
 | ||||
| class Pose2Graph{ | ||||
| 	Pose2Graph(); | ||||
| 	void print(string s) const; | ||||
| 	GaussianFactorGraph* linearize_(const Pose2Config& config) const; | ||||
| 	void push_back(Pose2Factor* factor); | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
							
								
								
									
										82
									
								
								cpp/gtsam.h
								
								
								
								
							
							
						
						
									
										82
									
								
								cpp/gtsam.h
								
								
								
								
							|  | @ -1,3 +1,12 @@ | |||
| class SharedGaussian { | ||||
| 		SharedGaussian(Matrix covariance); | ||||
| 		SharedGaussian(Vector sigmas); | ||||
| }; | ||||
| 
 | ||||
| class SharedDiagonal { | ||||
| 	SharedDiagonal(Vector sigmas); | ||||
| }; | ||||
| 
 | ||||
| class Ordering { | ||||
|   Ordering(); | ||||
|   Ordering(string key); | ||||
|  | @ -25,22 +34,17 @@ class VectorConfig { | |||
|   void clear(); | ||||
| }; | ||||
| 
 | ||||
| class GaussianFactorSet { | ||||
|   GaussianFactorSet(); | ||||
|   void push_back(GaussianFactor* factor); | ||||
| }; | ||||
| 
 | ||||
| class GaussianFactor { | ||||
|   GaussianFactor(string key1, | ||||
| 	       Matrix A1, | ||||
| 	       Vector b_in, | ||||
| 	       double sigma); | ||||
| 	       const SharedDiagonal& model); | ||||
|   GaussianFactor(string key1, | ||||
| 	       Matrix A1, | ||||
| 	       string key2, | ||||
| 	       Matrix A2, | ||||
| 	       Vector b_in, | ||||
| 	       double sigma); | ||||
| 	       const SharedDiagonal& model); | ||||
|   GaussianFactor(string key1, | ||||
| 	       Matrix A1, | ||||
| 	       string key2, | ||||
|  | @ -48,7 +52,7 @@ class GaussianFactor { | |||
| 	       string key3, | ||||
| 	       Matrix A3, | ||||
| 	       Vector b_in, | ||||
| 	       double sigma); | ||||
| 	       const SharedDiagonal& model); | ||||
|   bool empty() const; | ||||
|   Vector get_b() const; | ||||
|   Matrix get_A(string key) const; | ||||
|  | @ -59,6 +63,11 @@ class GaussianFactor { | |||
|   pair<Matrix,Vector> matrix(const Ordering& ordering) const; | ||||
| }; | ||||
| 
 | ||||
| class GaussianFactorSet { | ||||
|   GaussianFactorSet(); | ||||
|   void push_back(GaussianFactor* factor); | ||||
| }; | ||||
| 
 | ||||
| class GaussianConditional { | ||||
|   GaussianConditional(); | ||||
|   GaussianConditional(string key, | ||||
|  | @ -132,36 +141,6 @@ class Point3 { | |||
|   void print(string s) const; | ||||
| };  | ||||
| 
 | ||||
| class Point2Prior { | ||||
|   Point2Prior(Vector mu, double sigma, string key); | ||||
|   Vector error_vector(const VectorConfig& c) const; | ||||
|   GaussianFactor* linearize(const VectorConfig& c) const; | ||||
|   double sigma(); | ||||
|   Vector measurement(); | ||||
|   double error(const VectorConfig& c) const; | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Simulated2DOdometry { | ||||
|   Simulated2DOdometry(Vector odo, double sigma, string key, string key2); | ||||
|   Vector error_vector(const VectorConfig& c) const; | ||||
|   GaussianFactor* linearize(const VectorConfig& c) const; | ||||
|   double sigma(); | ||||
|   Vector measurement(); | ||||
|   double error(const VectorConfig& c) const; | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Simulated2DMeasurement { | ||||
|   Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); | ||||
|   Vector error_vector(const VectorConfig& c) const; | ||||
|   GaussianFactor* linearize(const VectorConfig& c) const; | ||||
|   double sigma(); | ||||
|   Vector measurement(); | ||||
|   double error(const VectorConfig& c) const; | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Pose2 { | ||||
|   Pose2(); | ||||
|   Pose2(const Pose2& pose); | ||||
|  | @ -179,30 +158,3 @@ class Pose2 { | |||
|   Point2 t() const; | ||||
|   Rot2 r() const; | ||||
| }; | ||||
| 
 | ||||
| class Pose2Config{ | ||||
| 	Pose2Config(); | ||||
| 	Pose2 get(string key) const; | ||||
| 	void insert(string name, const Pose2& val); | ||||
|     void print(string s) const; | ||||
|     void clear(); | ||||
|     int size(); | ||||
| }; | ||||
| 
 | ||||
| class Pose2Factor { | ||||
| 	Pose2Factor(string key1, string key2, | ||||
| 			const Pose2& measured, Matrix measurement_covariance); | ||||
| 	void print(string name) const; | ||||
| 	double error(const Pose2Config& c) const; | ||||
| 	size_t size() const; | ||||
| 	GaussianFactor* linearize(const Pose2Config& config) const; | ||||
| }; | ||||
| 
 | ||||
| class Pose2Graph{ | ||||
| 	Pose2Graph(); | ||||
| 	void print(string s) const; | ||||
| 	GaussianFactorGraph* linearize_(const Pose2Config& config) const; | ||||
| 	void push_back(Pose2Factor* factor); | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -26,19 +26,19 @@ namespace gtsam { | |||
| 		} | ||||
| 
 | ||||
| 		void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Odometry(i, j, z, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
| 
 | ||||
| 		void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Bearing(i, j, z, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
| 
 | ||||
| 		void Graph::addRange(const PoseKey& i, const PointKey& j, double z, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Range(i, j, z, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
|  |  | |||
|  | @ -35,7 +35,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		BearingFactor(); /* Default constructor */ | ||||
| 		BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, | ||||
| 				const sharedGaussian& model) : | ||||
| 				const SharedGaussian& model) : | ||||
| 			Base(model, i, j), z_(z) { | ||||
| 		} | ||||
| 
 | ||||
|  | @ -64,7 +64,7 @@ namespace gtsam { | |||
| 		RangeFactor(); /* Default constructor */ | ||||
| 
 | ||||
| 		RangeFactor(const PoseKey& i, const PointKey& j, double z, | ||||
| 				const sharedGaussian& model) : | ||||
| 				const SharedGaussian& model) : | ||||
| 			Base(model, i, j), z_(z) { | ||||
| 		} | ||||
| 
 | ||||
|  | @ -94,11 +94,11 @@ namespace gtsam { | |||
| 		struct Graph: public NonlinearFactorGraph<Config> { | ||||
| 			void addPoseConstraint(const PoseKey& i, const Pose2& p); | ||||
| 			void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, | ||||
| 					const sharedGaussian& model); | ||||
| 					const SharedGaussian& model); | ||||
| 			void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, | ||||
| 					const sharedGaussian& model); | ||||
| 					const SharedGaussian& model); | ||||
| 			void addRange(const PoseKey& i, const PointKey& j, double z, | ||||
| 					const sharedGaussian& model); | ||||
| 					const SharedGaussian& model); | ||||
| 		}; | ||||
| 
 | ||||
| 		// Optimizer
 | ||||
|  |  | |||
|  | @ -30,13 +30,13 @@ namespace gtsam { | |||
| 
 | ||||
| 		/* ************************************************************************* */ | ||||
| 		void Graph::addPrior(const Key& i, const Pose2& p, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Prior(i, p, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
| 
 | ||||
| 		void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Constraint(i, j, z, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
|  |  | |||
|  | @ -42,8 +42,8 @@ namespace gtsam { | |||
| 		struct Graph: public NonlinearFactorGraph<Config> { | ||||
| 			typedef BetweenFactor<Config, Key, Pose2> Constraint; | ||||
| 			typedef Pose2 Pose; | ||||
| 			void addPrior(const Key& i, const Pose2& p, const sharedGaussian& model); | ||||
| 			void addConstraint(const Key& i, const Key& j, const Pose2& z, const sharedGaussian& model); | ||||
| 			void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model); | ||||
| 			void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model); | ||||
| 			void addHardConstraint(const Key& i, const Pose2& p); | ||||
| 		}; | ||||
| 
 | ||||
|  |  | |||
|  | @ -33,13 +33,13 @@ namespace gtsam { | |||
| 
 | ||||
| 		/* ************************************************************************* */ | ||||
| 		void Graph::addPrior(const Key& i, const Pose3& p, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Prior(i, p, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
| 
 | ||||
| 		void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, | ||||
| 				const sharedGaussian& model) { | ||||
| 				const SharedGaussian& model) { | ||||
| 			sharedFactor factor(new Constraint(i, j, z, model)); | ||||
| 			push_back(factor); | ||||
| 		} | ||||
|  |  | |||
|  | @ -41,9 +41,9 @@ namespace gtsam { | |||
| 		// Graph
 | ||||
| 		struct Graph: public NonlinearFactorGraph<Config> { | ||||
| 			void addPrior(const Key& i, const Pose3& p, | ||||
| 					const sharedGaussian& model); | ||||
| 					const SharedGaussian& model); | ||||
| 			void addConstraint(const Key& i, const Key& j, const Pose3& z, | ||||
| 					const sharedGaussian& model); | ||||
| 					const SharedGaussian& model); | ||||
| 			void addHardConstraint(const Key& i, const Pose3& p); | ||||
| 		}; | ||||
| 
 | ||||
|  |  | |||
|  | @ -56,7 +56,7 @@ namespace gtsam { | |||
| 
 | ||||
| 			Point2 z_; | ||||
| 
 | ||||
| 			Prior(const Point2& z, const sharedGaussian& model, const PoseKey& key) : | ||||
| 			Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) : | ||||
| 				NonlinearFactor1<Config, PoseKey, Point2> (model, key), z_(z) { | ||||
| 			} | ||||
| 
 | ||||
|  | @ -74,7 +74,7 @@ namespace gtsam { | |||
| 				Point2> { | ||||
| 			Point2 z_; | ||||
| 
 | ||||
| 			Odometry(const Point2& z, const sharedGaussian& model, const PoseKey& j1, | ||||
| 			Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1, | ||||
| 					const PoseKey& j2) : | ||||
| 				z_(z), NonlinearFactor2<Config, PoseKey, Point2, PoseKey, Point2> ( | ||||
| 						model, j1, j2) { | ||||
|  | @ -95,7 +95,7 @@ namespace gtsam { | |||
| 
 | ||||
| 			Point2 z_; | ||||
| 
 | ||||
| 			Measurement(const Point2& z, const sharedGaussian& model, | ||||
| 			Measurement(const Point2& z, const SharedGaussian& model, | ||||
| 					const PoseKey& j1, const PointKey& j2) : | ||||
| 				z_(z), NonlinearFactor2<Config, PoseKey, Point2, PointKey, Point2> ( | ||||
| 						model, j1, j2) { | ||||
|  |  | |||
|  | @ -12,8 +12,6 @@ | |||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| // TODO: DANGEROUS, create shared pointers
 | ||||
| #define GTSAM_MAGIC_GAUSSIAN 2 | ||||
| #define GTSAM_MAGIC_KEY | ||||
| 
 | ||||
| #include "Ordering.h" | ||||
|  | @ -30,9 +28,10 @@ namespace example { | |||
| 
 | ||||
| 	typedef boost::shared_ptr<NonlinearFactor<Config> > shared; | ||||
| 
 | ||||
| 	static sharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); | ||||
| 	static sharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); | ||||
| 	static sharedDiagonal constraintModel = noiseModel::Constrained::All(2); | ||||
| 	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); | ||||
| 	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); | ||||
| 	static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); | ||||
| 	static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() { | ||||
|  | @ -41,27 +40,23 @@ namespace example { | |||
| 				new Graph); | ||||
| 
 | ||||
| 		// prior on x1
 | ||||
| 		double sigma1 = 0.1; | ||||
| 		Point2 mu; | ||||
| 		shared f1(new simulated2D::Prior(mu, sigma1, 1)); | ||||
| 		shared f1(new simulated2D::Prior(mu, sigma0_1, 1)); | ||||
| 		nlfg->push_back(f1); | ||||
| 
 | ||||
| 		// odometry between x1 and x2
 | ||||
| 		double sigma2 = 0.1; | ||||
| 		Point2 z2(1.5, 0); | ||||
| 		shared f2(new simulated2D::Odometry(z2, sigma2, 1, 2)); | ||||
| 		shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2)); | ||||
| 		nlfg->push_back(f2); | ||||
| 
 | ||||
| 		// measurement between x1 and l1
 | ||||
| 		double sigma3 = 0.2; | ||||
| 		Point2 z3(0, -1); | ||||
| 		shared f3(new simulated2D::Measurement(z3, sigma3, 1, 1)); | ||||
| 		shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1)); | ||||
| 		nlfg->push_back(f3); | ||||
| 
 | ||||
| 		// measurement between x2 and l1
 | ||||
| 		double sigma4 = 0.2; | ||||
| 		Point2 z4(-1.5, -1.); | ||||
| 		shared f4(new simulated2D::Measurement(z4, sigma4, 2, 1)); | ||||
| 		shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1)); | ||||
| 		nlfg->push_back(f4); | ||||
| 
 | ||||
| 		return nlfg; | ||||
|  | @ -193,7 +188,7 @@ namespace example { | |||
| 
 | ||||
| 			Point2 z_; | ||||
| 
 | ||||
| 			UnaryFactor(const Point2& z, const sharedGaussian& model, | ||||
| 			UnaryFactor(const Point2& z, const SharedGaussian& model, | ||||
| 					const simulated2D::PoseKey& key) : | ||||
| 				gtsam::NonlinearFactor1<Config, simulated2D::PoseKey, | ||||
| 						Point2>(model, key), z_(z) { | ||||
|  | @ -228,28 +223,25 @@ namespace example { | |||
| 	/* ************************************************************************* */ | ||||
| 	pair<Graph, Config> createNonlinearSmoother(int T) { | ||||
| 
 | ||||
| 		// noise on measurements and odometry, respectively
 | ||||
| 		double sigma1 = 1, sigma2 = 1; | ||||
| 
 | ||||
| 		// Create
 | ||||
| 		Graph nlfg; | ||||
| 		Config poses; | ||||
| 
 | ||||
| 		// prior on x1
 | ||||
| 		Point2 x1(1.0, 0.0); | ||||
| 		shared prior(new simulated2D::Prior(x1, sigma1, 1)); | ||||
| 		shared prior(new simulated2D::Prior(x1, sigma1_0, 1)); | ||||
| 		nlfg.push_back(prior); | ||||
| 		poses.insert(simulated2D::PoseKey(1), x1); | ||||
| 
 | ||||
| 		for (int t = 2; t <= T; t++) { | ||||
| 			// odometry between x_t and x_{t-1}
 | ||||
| 			Point2 odo(1.0, 0.0); | ||||
| 			shared odometry(new simulated2D::Odometry(odo, sigma2, t - 1, t)); | ||||
| 			shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t)); | ||||
| 			nlfg.push_back(odometry); | ||||
| 
 | ||||
| 			// measurement on x_t is like perfect GPS
 | ||||
| 			Point2 xt(t, 0); | ||||
| 			shared measurement(new simulated2D::Prior(xt, sigma1, t)); | ||||
| 			shared measurement(new simulated2D::Prior(xt, sigma1_0, t)); | ||||
| 			nlfg.push_back(measurement); | ||||
| 
 | ||||
| 			// initial estimate
 | ||||
|  | @ -524,17 +516,14 @@ namespace example { | |||
| 		NonlinearFactorGraph<Config> nlfg; | ||||
| 
 | ||||
| 		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
 | ||||
| 		double sigma0 = 1e-3; | ||||
| 		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sigma0, key(1,1))); | ||||
| 		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1))); | ||||
| 		nlfg.push_back(constraint); | ||||
| 
 | ||||
| 		double sigma = 0.01; | ||||
| 
 | ||||
| 		// Create horizontal constraints, 1...N*(N-1)
 | ||||
| 		Point2 z1(1.0, 0.0); // move right
 | ||||
| 		for (size_t x = 1; x < N; x++) | ||||
| 			for (size_t y = 1; y <= N; y++) { | ||||
| 				shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y))); | ||||
| 				shared f(new simulated2D::Odometry(z1, sharedSigma(2,0.01), key(x, y), key(x + 1, y))); | ||||
| 				nlfg.push_back(f); | ||||
| 			} | ||||
| 
 | ||||
|  | @ -542,7 +531,7 @@ namespace example { | |||
| 		Point2 z2(0.0, 1.0); // move up
 | ||||
| 		for (size_t x = 1; x <= N; x++) | ||||
| 			for (size_t y = 1; y < N; y++) { | ||||
| 				shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1))); | ||||
| 				shared f(new simulated2D::Odometry(z2, sharedSigma(2,0.01), key(x, y), key(x, y + 1))); | ||||
| 				nlfg.push_back(f); | ||||
| 			} | ||||
| 
 | ||||
|  |  | |||
|  | @ -27,7 +27,7 @@ using namespace gtsam; | |||
| using namespace example; | ||||
| using namespace boost; | ||||
| 
 | ||||
| static sharedDiagonal | ||||
| static SharedDiagonal | ||||
| 	sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), | ||||
| 	constraintModel = noiseModel::Constrained::All(2); | ||||
| 
 | ||||
|  | @ -225,7 +225,7 @@ TEST( NonlinearFactorGraph, combine2){ | |||
| TEST( GaussianFactor, linearFactorN){ | ||||
| 	Matrix I = eye(2); | ||||
|   vector<GaussianFactor::shared_ptr> f; | ||||
|   sharedDiagonal model = sharedSigma(2,1.0); | ||||
|   SharedDiagonal model = sharedSigma(2,1.0); | ||||
|   f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2, | ||||
| 			10.0, 5.0), model))); | ||||
| 	f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I, | ||||
|  |  | |||
|  | @ -263,7 +263,7 @@ TEST( GaussianFactorGraph, add_priors ) | |||
|   GaussianFactorGraph expected = createGaussianFactorGraph(); | ||||
|   Matrix A = eye(2); | ||||
|   Vector b = zero(2); | ||||
|   sharedDiagonal sigma = sharedSigma(2,3.0); | ||||
|   SharedDiagonal sigma = sharedSigma(2,3.0); | ||||
|   expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma))); | ||||
|   expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma))); | ||||
|   expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma))); | ||||
|  | @ -629,7 +629,7 @@ TEST( GaussianFactorGraph, elimination ) | |||
| 	GaussianFactorGraph fg; | ||||
| 	Matrix Ap = eye(1), An = eye(1) * -1; | ||||
| 	Vector b = Vector_(1, 0.0); | ||||
|   sharedDiagonal sigma = sharedSigma(2,2.0); | ||||
|   SharedDiagonal sigma = sharedSigma(2,2.0); | ||||
| 	fg.add("x1", An, "x2", Ap, b, sigma); | ||||
| 	fg.add("x1", Ap, b, sigma); | ||||
| 	fg.add("x2", Ap, b, sigma); | ||||
|  | @ -735,7 +735,7 @@ TEST( GaussianFactorGraph, constrained_multi2 ) | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| sharedDiagonal model = sharedSigma(2,1); | ||||
| SharedDiagonal model = sharedSigma(2,1); | ||||
| 
 | ||||
| TEST( GaussianFactorGraph, findMinimumSpanningTree ) | ||||
| { | ||||
|  |  | |||
|  | @ -10,7 +10,7 @@ using namespace boost::assign; | |||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| // TODO: DANGEROUS, create shared pointers
 | ||||
| #define GTSAM_MAGIC_GAUSSIAN 3 | ||||
| #define GTSAM_DANGEROUS_GAUSSIAN 3 | ||||
| #define GTSAM_MAGIC_KEY | ||||
| 
 | ||||
| #include "Ordering.h" | ||||
|  | @ -114,8 +114,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) | |||
| 	config.insert(2, Pose2(1.5,0.,0.)); | ||||
| 
 | ||||
| 	Pose2Graph graph; | ||||
| 	graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); | ||||
| 	graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); | ||||
| 	graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); | ||||
| 	graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); | ||||
| 
 | ||||
| 	VectorConfig zeros; | ||||
| 	zeros.insert("x1",zero(3)); | ||||
|  | @ -140,8 +140,8 @@ TEST( Iterative, subgraphPCG ) | |||
| 	theta_bar.insert(2, Pose2(1.5,0.,0.)); | ||||
| 
 | ||||
| 	Pose2Graph graph; | ||||
| 	graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); | ||||
| 	graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); | ||||
| 	graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); | ||||
| 	graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); | ||||
| 
 | ||||
| 	VectorConfig zeros; | ||||
| 	zeros.insert("x1",zero(3)); | ||||
|  |  | |||
|  | @ -11,7 +11,6 @@ | |||
| #include <boost/foreach.hpp> | ||||
| #include <boost/numeric/ublas/matrix_proxy.hpp> | ||||
| #include "Matrix.h" | ||||
| #include "NoiseModel.h" | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  | @ -419,6 +418,8 @@ TEST( matrix, inverse ) | |||
|   A(2,0)= 1;  A(2,1)=0; A(2,2)=6; | ||||
| 
 | ||||
|   Matrix Ainv = inverse(A); | ||||
|   CHECK(assert_equal(eye(3), A*Ainv)); | ||||
|   CHECK(assert_equal(eye(3), Ainv*A)); | ||||
| 
 | ||||
|   Matrix expected(3,3); | ||||
|   expected(0,0)= 1.0909;   expected(0,1)=-0.5454; expected(0,2)=-0.0909; | ||||
|  | @ -426,6 +427,26 @@ TEST( matrix, inverse ) | |||
|   expected(2,0)= -0.1818;  expected(2,1)= 0.0909; expected(2,2)=0.1818; | ||||
| 
 | ||||
|   CHECK(assert_equal(expected, Ainv, 1e-4)); | ||||
| 
 | ||||
|   // These two matrices failed before version 2003 because we called LU incorrectly
 | ||||
|   Matrix lMg(Matrix_(3,3, | ||||
|   		0.0,  1.0,-2.0, | ||||
|   	 -1.0,  0.0, 1.0, | ||||
|   		0.0,  0.0, 1.0)); | ||||
|   CHECK(assert_equal(Matrix_(3,3, | ||||
|   		0.0, -1.0, 1.0, | ||||
|   		1.0,  0.0, 2.0, | ||||
|   		0.0,  0.0, 1.0), | ||||
|   		inverse(lMg))); | ||||
|   Matrix gMl(Matrix_(3,3, | ||||
|   		0.0, -1.0, 1.0, | ||||
|   		1.0,  0.0, 2.0, | ||||
|   		0.0,  0.0, 1.0)); | ||||
|   CHECK(assert_equal(Matrix_(3,3, | ||||
|   		0.0,  1.0,-2.0, | ||||
|   	 -1.0,  0.0, 1.0, | ||||
|   		0.0,  0.0, 1.0), | ||||
|   		inverse(gMl))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -623,69 +644,6 @@ static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, | |||
| 	} | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| list<boost::tuple<Vector, double, double> > | ||||
| weighted_eliminate2(Matrix& A, Vector& b, const sharedGaussian& model) { | ||||
| 	size_t m = A.size1(), n = A.size2(); // get size(A)
 | ||||
| 	size_t maxRank = min(m,n); | ||||
| 
 | ||||
| 	// pre-whiten everything
 | ||||
| 	model->WhitenInPlace(A); | ||||
| 	b = model->whiten(b); | ||||
| 
 | ||||
| 	// create list
 | ||||
| 	list<boost::tuple<Vector, double, double> > results; | ||||
| 
 | ||||
| 	// We loop over all columns, because the columns that can be eliminated
 | ||||
| 	// are not necessarily contiguous. For each one, estimate the corresponding
 | ||||
| 	// scalar variable x as d-rS, with S the separator (remaining columns).
 | ||||
| 	// Then update A and b by substituting x with d-rS, zero-ing out x's column.
 | ||||
| 	for (int j=0; j<n; ++j) { | ||||
| 		// extract the first column of A
 | ||||
| 		Vector a(column(A, j)); // ublas::matrix_column is slower !
 | ||||
| 
 | ||||
| 		// Calculate weighted pseudo-inverse and corresponding precision
 | ||||
| 		double precision = dot(a,a); | ||||
| 		Vector pseudo = a/precision; | ||||
| 
 | ||||
| 		// if precision is zero, no information on this column
 | ||||
| 		if (precision < 1e-8) continue; | ||||
| 
 | ||||
| 		// create solution and copy into r
 | ||||
| 		Vector r(basis(n, j)); | ||||
| 		for (int j2=j+1; j2<n; ++j2) // expensive !!
 | ||||
| 			r(j2) = inner_prod(pseudo, boost::numeric::ublas::matrix_column<Matrix>(A, j2)); | ||||
| 
 | ||||
| 		// create the rhs
 | ||||
| 		double d = inner_prod(pseudo, b); | ||||
| 
 | ||||
| 		// construct solution (r, d, sigma)
 | ||||
| 		// TODO: avoid sqrt, store precision or at least variance
 | ||||
| 		results.push_back(boost::make_tuple(r, d, 1./sqrt(precision))); | ||||
| 
 | ||||
| 		// exit after rank exhausted
 | ||||
| 		if (results.size()>=maxRank) break; | ||||
| 
 | ||||
| 		// update A, b, expensive, suing outer product
 | ||||
| 		// A' \define A_{S}-a*r and b'\define b-d*a
 | ||||
| 		updateAb(A, b, j, a, r, d); | ||||
| 	} | ||||
| 
 | ||||
| 	return results; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void weighted_eliminate3(Matrix& A, Vector& b, const sharedGaussian& model) { | ||||
| 	size_t m = A.size1(), n = A.size2(); // get size(A)
 | ||||
| 	size_t maxRank = min(m,n); | ||||
| 
 | ||||
| 	// pre-whiten everything
 | ||||
| 	model->WhitenInPlace(A); | ||||
| 	b = model->whiten(b); | ||||
| 
 | ||||
| 	householder_(A, maxRank); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( matrix, weighted_elimination ) | ||||
| { | ||||
|  | @ -727,27 +685,6 @@ TEST( matrix, weighted_elimination ) | |||
| 		DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5);  // verify sigma
 | ||||
| 		i += 1; | ||||
| 	} | ||||
| 
 | ||||
| 	// perform elimination with NoiseModel
 | ||||
| 	Matrix A2 = A; Vector b2 = b; | ||||
| 	sharedGaussian model = noiseModel::Diagonal::Sigmas(sigmas); | ||||
| 	std::list<boost::tuple<Vector, double, double> > solution2 = | ||||
| 								weighted_eliminate2(A2, b2, model); | ||||
| 
 | ||||
| 	// unpack and verify
 | ||||
| 	i=0; | ||||
| 	BOOST_FOREACH(boost::tie(r, di, sigma), solution2) { | ||||
| 		CHECK(assert_equal(r, row(expectedR, i))); // verify r
 | ||||
| 		DOUBLES_EQUAL(d(i), di, 1e-8);             // verify d
 | ||||
| 		DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5);  // verify sigma
 | ||||
| 		i += 1; | ||||
| 	} | ||||
| 
 | ||||
| 	// perform elimination with NoiseModel
 | ||||
| 	weighted_eliminate3(A, b, model); | ||||
| 	sharedGaussian newModel = noiseModel::Diagonal::Sigmas(newSigmas); | ||||
| //	print(A);
 | ||||
| //	print(newModel->Whiten(expectedR));
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -11,6 +11,8 @@ | |||
| #include <boost/foreach.hpp> | ||||
| #include <iostream> | ||||
| #include "NoiseModel.h" | ||||
| #include "SharedGaussian.h" | ||||
| #include "SharedDiagonal.h" | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  | @ -148,12 +150,12 @@ TEST(NoiseModel, ConstrainedAll ) | |||
| //
 | ||||
| //	// Expected result
 | ||||
| //	Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
 | ||||
| //	sharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
 | ||||
| //	SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
 | ||||
| //
 | ||||
| //	// Call Gaussian version
 | ||||
| //	sharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
 | ||||
| //	sharedDiagonal actual1 = diagonal->QR(Ab1);
 | ||||
| //	sharedDiagonal expected = noiseModel::Unit::Create(4);
 | ||||
| //	SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
 | ||||
| //	SharedDiagonal actual1 = diagonal->QR(Ab1);
 | ||||
| //	SharedDiagonal expected = noiseModel::Unit::Create(4);
 | ||||
| //	CHECK(assert_equal(*expected,*actual1));
 | ||||
| //	Matrix expectedRd1 = Matrix_(4, 6+1,
 | ||||
| //			11.1803,   0.0,   -2.23607, 0.0,    -8.94427, 0.0,     2.23607,
 | ||||
|  | @ -163,8 +165,8 @@ TEST(NoiseModel, ConstrainedAll ) | |||
| //	CHECK(assert_equal(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!!
 | ||||
| //
 | ||||
| //	// Call Constrained version
 | ||||
| //	sharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
 | ||||
| //	sharedDiagonal actual2 = constrained->QR(Ab2);
 | ||||
| //	SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
 | ||||
| //	SharedDiagonal actual2 = constrained->QR(Ab2);
 | ||||
| //	CHECK(assert_equal(*expectedModel,*actual2));
 | ||||
| //	Matrix expectedRd2 = Matrix_(4, 6+1,
 | ||||
| //			1.,  0., -0.2,  0., -0.8, 0.,  0.2,
 | ||||
|  | @ -177,13 +179,13 @@ TEST(NoiseModel, ConstrainedAll ) | |||
| /* ************************************************************************* */ | ||||
| TEST(NoiseModel, QRNan ) | ||||
| { | ||||
| 	sharedDiagonal constrained = noiseModel::Constrained::All(2); | ||||
| 	SharedDiagonal constrained = noiseModel::Constrained::All(2); | ||||
| 	Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.); | ||||
| 
 | ||||
| 	sharedDiagonal expected = noiseModel::Constrained::All(2); | ||||
| 	SharedDiagonal expected = noiseModel::Constrained::All(2); | ||||
| 	Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); | ||||
| 
 | ||||
| 	sharedDiagonal actual = constrained->QR(Ab); | ||||
| 	SharedDiagonal actual = constrained->QR(Ab); | ||||
| 	CHECK(assert_equal(*expected,*actual)); | ||||
| 	CHECK(assert_equal(expectedAb,Ab)); | ||||
| } | ||||
|  | @ -192,8 +194,8 @@ TEST(NoiseModel, QRNan ) | |||
| TEST(NoiseModel, SmartCovariance ) | ||||
| { | ||||
| 	bool smart = true; | ||||
| 	sharedGaussian expected = Unit::Create(3); | ||||
| 	sharedGaussian actual = Gaussian::Covariance(eye(3), smart); | ||||
| 	SharedGaussian expected = Unit::Create(3); | ||||
| 	SharedGaussian actual = Gaussian::Covariance(eye(3), smart); | ||||
| 	CHECK(assert_equal(*expected,*actual)); | ||||
| } | ||||
| 
 | ||||
|  | @ -201,8 +203,8 @@ TEST(NoiseModel, SmartCovariance ) | |||
| TEST(NoiseModel, ScalarOrVector ) | ||||
| { | ||||
| 	bool smart = true; | ||||
| 	sharedGaussian expected = Unit::Create(3); | ||||
| 	sharedGaussian actual = Gaussian::Covariance(eye(3), smart); | ||||
| 	SharedGaussian expected = Unit::Create(3); | ||||
| 	SharedGaussian actual = Gaussian::Covariance(eye(3), smart); | ||||
| 	CHECK(assert_equal(*expected,*actual)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -87,9 +87,9 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { | |||
| 	boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); | ||||
| 
 | ||||
| 	// verify
 | ||||
| 	sharedDiagonal probModel = sharedSigma(p,1.0); | ||||
| 	SharedDiagonal probModel = sharedSigma(p,1.0); | ||||
| 	GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); | ||||
| 	sharedDiagonal constraintModel = noiseModel::Constrained::All(p); | ||||
| 	SharedDiagonal constraintModel = noiseModel::Constrained::All(p); | ||||
| 	GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); | ||||
| 	CHECK(assert_equal(*actualFactor, expectedFactor)); | ||||
| 	CHECK(assert_equal(*actualConstraint, expectedConstraint)); | ||||
|  | @ -188,11 +188,11 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { | |||
| 	boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); | ||||
| 
 | ||||
| 	// verify
 | ||||
| 	sharedDiagonal probModel = sharedSigma(p,1.0); | ||||
| 	SharedDiagonal probModel = sharedSigma(p,1.0); | ||||
| 	GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), | ||||
| 							 x1, Matrix_(1,1, -3.0), | ||||
| 							 "L12", eye(1), zero(1), probModel); | ||||
| 	sharedDiagonal constraintModel = noiseModel::Constrained::All(p); | ||||
| 	SharedDiagonal constraintModel = noiseModel::Constrained::All(p); | ||||
| 	GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), | ||||
| 								 x1, Matrix_(1,1, -1.0), | ||||
| 								 Vector_(1, 6.0), constraintModel); | ||||
|  | @ -289,9 +289,9 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { | |||
| 	CHECK(c1.active(config2)); | ||||
| 
 | ||||
| 	// verify
 | ||||
| 	sharedDiagonal probModel = sharedSigma(p,1.0); | ||||
| 	SharedDiagonal probModel = sharedSigma(p,1.0); | ||||
| 	GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); | ||||
| 	sharedDiagonal constraintModel = noiseModel::Constrained::All(p); | ||||
| 	SharedDiagonal constraintModel = noiseModel::Constrained::All(p); | ||||
| 	GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); | ||||
| 	CHECK(assert_equal(*actualFactor2, expectedFactor)); | ||||
| 	CHECK(assert_equal(*actualConstraint2, expectedConstraint)); | ||||
|  |  | |||
|  | @ -31,7 +31,7 @@ TEST ( NonlinearEquality, linearization ) { | |||
| 	shared_nle nle(new NLE(key, value,vector_compare)); | ||||
| 
 | ||||
| 	// check linearize
 | ||||
| 	sharedDiagonal constraintModel = noiseModel::Constrained::All(2); | ||||
| 	SharedDiagonal constraintModel = noiseModel::Constrained::All(2); | ||||
| 	GaussianFactor expLF(key, eye(2), zero(2), constraintModel); | ||||
| 	GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); | ||||
| 	CHECK(assert_equal(*actualLF, expLF)); | ||||
|  |  | |||
|  | @ -29,7 +29,7 @@ typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf; | |||
| /* ************************************************************************* */ | ||||
| TEST( NonlinearFactor, equals ) | ||||
| { | ||||
| 	sharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0)); | ||||
| 	SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0)); | ||||
| 
 | ||||
| 	// create two nonlinear2 factors
 | ||||
| 	Point2 z3(0.,-1.); | ||||
|  |  | |||
|  | @ -170,8 +170,8 @@ TEST( NonlinearOptimizer, Factorization ) | |||
| 	config->insert(2, Pose2(1.5,0.,0.)); | ||||
| 
 | ||||
| 	boost::shared_ptr<Pose2Graph> graph(new Pose2Graph); | ||||
| 	graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); | ||||
| 	graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); | ||||
| 	graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); | ||||
| 	graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); | ||||
| 
 | ||||
| 	boost::shared_ptr<Ordering> ordering(new Ordering); | ||||
| 	ordering->push_back(Pose2Config::Key(1)); | ||||
|  | @ -197,8 +197,8 @@ TEST( NonlinearOptimizer, SubgraphPCG ) | |||
| 	config->insert(2, Pose2(1.5,0.,0.)); | ||||
| 
 | ||||
| 	boost::shared_ptr<Pose2Graph> graph(new Pose2Graph); | ||||
| 	graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); | ||||
| 	graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); | ||||
| 	graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); | ||||
| 	graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); | ||||
| 
 | ||||
| 	double relativeThreshold = 1e-5; | ||||
| 	double absoluteThreshold = 1e-5; | ||||
|  |  | |||
|  | @ -15,7 +15,7 @@ using namespace gtsam; | |||
| static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); | ||||
| static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); | ||||
| 
 | ||||
| sharedGaussian | ||||
| SharedGaussian | ||||
| 	sigma(noiseModel::Isotropic::Sigma(1,0.1)), | ||||
| 	I3(noiseModel::Unit::Create(3)); | ||||
| 
 | ||||
|  |  | |||
|  | @ -181,15 +181,15 @@ Matrix matrix(const Pose2& gTl) { | |||
| TEST( Pose2, matrix ) | ||||
| { | ||||
| 	Point2 origin, t(1,2); | ||||
| 	Pose2 gT1(M_PI_2, t); // robot at (1,2) looking towards y
 | ||||
|   Matrix gM1 = matrix(gT1); | ||||
| 	Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y
 | ||||
|   Matrix gMl = matrix(gTl); | ||||
|   CHECK(assert_equal(Matrix_(3,3, | ||||
|   		0.0, -1.0, 1.0, | ||||
|   		1.0,  0.0, 2.0, | ||||
|   		0.0,  0.0, 1.0), | ||||
|   		gM1)); | ||||
|   Rot2 gR1 = gT1.r(); | ||||
|   CHECK(assert_equal(homogeneous(t),gM1*homogeneous(origin))); | ||||
|   		gMl)); | ||||
|   Rot2 gR1 = gTl.r(); | ||||
|   CHECK(assert_equal(homogeneous(t),gMl*homogeneous(origin))); | ||||
|   Point2 x_axis(1,0), y_axis(0,1); | ||||
|   CHECK(assert_equal(Matrix_(2,2, | ||||
|   		0.0, -1.0, | ||||
|  | @ -197,14 +197,26 @@ TEST( Pose2, matrix ) | |||
|   		gR1.matrix())); | ||||
|   CHECK(assert_equal(Point2(0,1),gR1*x_axis)); | ||||
|   CHECK(assert_equal(Point2(-1,0),gR1*y_axis)); | ||||
|   CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gM1*homogeneous(x_axis))); | ||||
|   CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gM1*homogeneous(y_axis))); | ||||
|   CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis))); | ||||
|   CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis))); | ||||
| 
 | ||||
|   // check inverse pose
 | ||||
|   Matrix _1Mg = matrix(inverse(gT1)); | ||||
|   CHECK(assert_equal(inverse(gM1),_1Mg)); | ||||
|   CHECK(assert_equal(eye(3),inverse(_1Mg)*_1Mg)); | ||||
|   CHECK(assert_equal(eye(3),inverse(gM1)*gM1)); | ||||
|   Matrix lMg = matrix(inverse(gTl)); | ||||
|   CHECK(assert_equal(Matrix_(3,3, | ||||
|   		0.0,  1.0,-2.0, | ||||
|   	 -1.0,  0.0, 1.0, | ||||
|   		0.0,  0.0, 1.0), | ||||
|   		lMg)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( Pose2, compose_matrix ) | ||||
| { | ||||
|   Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
 | ||||
|   Pose2 _1T2(M_PI, Point2(-1,4));  // local robot at (-1,4) loooking at negative x
 | ||||
|   Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); | ||||
|   CHECK(assert_equal(gM1*_1M2,matrix(compose(_1T2,gT1)))); // WRONG CHECKS OUT !
 | ||||
|   // CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); RIGHT DOES NOT
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -221,13 +233,6 @@ TEST( Pose2, between ) | |||
|   Matrix actualH1,actualH2; | ||||
|   Pose2 expected(M_PI_2, Point2(2,2)); | ||||
|   Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!!
 | ||||
|   GTSAM_PRINT(between(gT1,gT2)); | ||||
|   // what we want is 1T2 = 1Tg * gT2 = between(2Tg,1Tg)
 | ||||
| //  print(matrix(gT1));
 | ||||
| //  print(matrix(gT2));
 | ||||
| //  print(inverse(matrix(gT1))*matrix(gT2)); // 1T2
 | ||||
| //  print(inverse(matrix(gT2))*matrix(gT1)); // 2T1
 | ||||
| //  GTSAM_PRINT(between(inverse(gT2),inverse(gT1)));
 | ||||
|   Pose2 actual2 = between(gT1,gT2,actualH1,actualH2); | ||||
|   CHECK(assert_equal(expected,actual1)); | ||||
|   CHECK(assert_equal(expected,actual2)); | ||||
|  |  | |||
|  | @ -114,7 +114,7 @@ TEST( Pose2Factor, linearize ) | |||
| 	Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); | ||||
| 
 | ||||
| 	// expected linear factor
 | ||||
| 	sharedDiagonal probModel1 = noiseModel::Unit::Create(3); | ||||
| 	SharedDiagonal probModel1 = noiseModel::Unit::Create(3); | ||||
| 	GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, probModel1); | ||||
| 
 | ||||
| 	// Actual linearization
 | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ using namespace gtsam; | |||
| 
 | ||||
| // Common measurement covariance
 | ||||
| static double sx=0.5, sy=0.5,st=0.1; | ||||
| static sharedGaussian sigmas = Diagonal::Sigmas(Vector_(3,sx,sy,st)); | ||||
| static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st)); | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Very simple test establishing Ax-b \approx z-h(x)
 | ||||
|  |  | |||
|  | @ -79,7 +79,7 @@ TEST( Pose2Graph, linearization ) | |||
| 
 | ||||
| 	double sigma = 1; | ||||
| 	Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); | ||||
| 	sharedDiagonal probModel1 = noiseModel::Unit::Create(3); | ||||
| 	SharedDiagonal probModel1 = noiseModel::Unit::Create(3); | ||||
| 	lfg_expected.add("x1", A1, "x2", A2, b, probModel1); | ||||
| 
 | ||||
| 	CHECK(assert_equal(lfg_expected, lfg_linearized)); | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ TEST( Pose3Factor, error ) | |||
| 	Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; | ||||
| 
 | ||||
| 	// Create factor
 | ||||
| 	sharedGaussian I6(noiseModel::Unit::Create(6)); | ||||
| 	SharedGaussian I6(noiseModel::Unit::Create(6)); | ||||
| 	Pose3Factor factor(1,2, z, I6); | ||||
| 
 | ||||
| 	// Create config
 | ||||
|  |  | |||
|  | @ -13,7 +13,7 @@ | |||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| // TODO: DANGEROUS, create shared pointers
 | ||||
| #define GTSAM_MAGIC_GAUSSIAN 2 | ||||
| #define GTSAM_DANGEROUS_GAUSSIAN 2 | ||||
| #define GTSAM_MAGIC_KEY | ||||
| 
 | ||||
| #include <Pose3.h> | ||||
|  | @ -36,9 +36,9 @@ using namespace boost; | |||
| using namespace boost::assign; | ||||
| 
 | ||||
| // Models to use
 | ||||
| sharedDiagonal probModel1 = sharedSigma(1,1.0); | ||||
| sharedDiagonal probModel2 = sharedSigma(2,1.0); | ||||
| sharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); | ||||
| SharedDiagonal probModel1 = sharedSigma(1,1.0); | ||||
| SharedDiagonal probModel2 = sharedSigma(2,1.0); | ||||
| SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); | ||||
| 
 | ||||
| // trick from some reading group
 | ||||
| #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) | ||||
|  | @ -279,7 +279,7 @@ TEST (SQP, two_pose_truth ) { | |||
| 
 | ||||
| 	// measurement from x1 to l1
 | ||||
| 	Point2 z1(0.0, 5.0); | ||||
| 	sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); | ||||
| 	SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); | ||||
| 	shared f1(new simulated2D::Measurement(z1, sigma, x1,l1)); | ||||
| 	graph->push_back(f1); | ||||
| 
 | ||||
|  | @ -387,7 +387,7 @@ TEST (SQP, two_pose ) { | |||
| 
 | ||||
| 	// measurement from x1 to l1
 | ||||
| 	Point2 z1(0.0, 5.0); | ||||
| 	sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); | ||||
| 	SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); | ||||
| 	shared f1(new simulated2D::Measurement(z1, sigma, x1,l1)); | ||||
| 	graph->push_back(f1); | ||||
| 
 | ||||
|  |  | |||
|  | @ -29,7 +29,7 @@ using namespace boost; | |||
| using namespace boost::assign; | ||||
| using namespace simulated2D; | ||||
| 	 | ||||
| static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); | ||||
| static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); | ||||
| 
 | ||||
| // typedefs
 | ||||
| typedef simulated2D::Config Config2D; | ||||
|  |  | |||
|  | @ -24,7 +24,7 @@ static double fov = 60; // degrees | |||
| static size_t w=640,h=480; | ||||
| static Cal3_S2 K(fov,w,h); | ||||
| 
 | ||||
| static sharedGaussian sigma(noiseModel::Unit::Create(1)); | ||||
| static SharedGaussian sigma(noiseModel::Unit::Create(1)); | ||||
| static shared_ptrK sK(new Cal3_S2(K)); | ||||
| 
 | ||||
| // make cameras
 | ||||
|  | @ -52,7 +52,7 @@ TEST( ProjectionFactor, error ) | |||
|   Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.); | ||||
|   Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.); | ||||
|   Vector b = Vector_(2,3.,0.); | ||||
|   sharedDiagonal probModel1 = noiseModel::Unit::Create(2); | ||||
|   SharedDiagonal probModel1 = noiseModel::Unit::Create(2); | ||||
|   GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1); | ||||
|   GaussianFactor::shared_ptr actual = factor->linearize(config); | ||||
|   CHECK(assert_equal(expected,*actual,1e-3)); | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ using namespace gtsam; | |||
| using namespace gtsam::visualSLAM; | ||||
| using namespace boost; | ||||
| typedef NonlinearOptimizer<Graph,Config> Optimizer; | ||||
| static sharedGaussian sigma(noiseModel::Unit::Create(1)); | ||||
| static SharedGaussian sigma(noiseModel::Unit::Create(1)); | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 landmark1(-1.0,-1.0, 0.0); | ||||
|  |  | |||
|  | @ -63,7 +63,7 @@ namespace gtsam { namespace visualSLAM { | |||
|      * @param K the constant calibration | ||||
|      */ | ||||
|     ProjectionFactor(const Point2& z, | ||||
| 					const sharedGaussian& model, PoseKey j_pose, | ||||
| 					const SharedGaussian& model, PoseKey j_pose, | ||||
| 					PointKey j_landmark, const shared_ptrK& K) : | ||||
| 				z_(z), K_(K), Base(model, j_pose, j_landmark) { | ||||
| 			} | ||||
|  |  | |||
|  | @ -6,46 +6,36 @@ c = createNoisyConfig(); | |||
| 
 | ||||
| % Create | ||||
| fg = GaussianFactorGraph; | ||||
| sigma1=.1; | ||||
| 
 | ||||
| % Create shared Noise models | ||||
| sigma0_1 = SharedDiagonal([0.1;0.1]); | ||||
| sigma0_2 = SharedDiagonal([0.2;0.2]); | ||||
| 
 | ||||
| % prior on x1 | ||||
| A11=eye(2); | ||||
| b = - c.get('x1'); | ||||
| 
 | ||||
| f1 = GaussianFactor('x1', A11, b, sigma1); % generate a Gaussian factor of odometry | ||||
| f1 = GaussianFactor('x1', A11, b, sigma0_1); % generate a Gaussian factor of odometry | ||||
| fg.push_back(f1); | ||||
| 
 | ||||
| % odometry between x1 and x2 | ||||
| sigma2=.1; | ||||
| 
 | ||||
| A21=-eye(2); | ||||
| A22=eye(2); | ||||
| b = [.2;-.1]; | ||||
| 
 | ||||
| f2 = GaussianFactor('x1', A21,  'x2', A22, b,sigma2); | ||||
| 
 | ||||
| f2 = GaussianFactor('x1', A21,  'x2', A22, b,sigma0_1); | ||||
| fg.push_back(f2); | ||||
| 
 | ||||
| % measurement between x1 and l1 | ||||
| sigma3=.2; | ||||
| A31=-eye(2); | ||||
| A33=eye(2); | ||||
| b = [0;.2]; | ||||
| 
 | ||||
| 
 | ||||
| f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma3); | ||||
| 
 | ||||
| f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma0_2); | ||||
| fg.push_back(f3); | ||||
| 
 | ||||
| % measurement between x2 and l1 | ||||
| sigma4=.2; | ||||
| A42=-eye(2); | ||||
| A43=eye(2); | ||||
| b = [-.2;.3]; | ||||
| 
 | ||||
| 
 | ||||
| f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma4); | ||||
| 
 | ||||
| f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma0_2); | ||||
| fg.push_back(f4); | ||||
| 
 | ||||
| end | ||||
|  | @ -25,14 +25,13 @@ Ax1 = [ | |||
| 
 | ||||
| % the RHS | ||||
| b2=[-1;1.5;2;-1]; | ||||
| 
 | ||||
| combined = GaussianFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, 1); | ||||
| model = SharedDiagonal([1;1]); | ||||
| combined = GaussianFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, model); | ||||
| 
 | ||||
| % eliminate the combined factor | ||||
| % NOT WORKING | ||||
| % this is not working because there is no elimination function for a linear | ||||
| % factor. Just for the MutableGaussianFactor | ||||
| %[actualCG,actualLF] = combined.eliminate('x2'); | ||||
| % this is not working because the eliminate has to be added back to gtsam.h | ||||
| [actualCG,actualLF] = combined.eliminate('x2'); | ||||
| 
 | ||||
| % create expected Conditional Gaussian | ||||
| R11 = [ | ||||
|  | @ -48,7 +47,7 @@ S13 = [ | |||
| +0.00,-8.94427 | ||||
| ]; | ||||
| d=[2.23607;-1.56525]; | ||||
| expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1 1]'); | ||||
| expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,model); | ||||
| 
 | ||||
| % the expected linear factor | ||||
| Bl1 = [ | ||||
|  | @ -70,5 +69,5 @@ expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, 1); | |||
| % check if the result matches | ||||
| % NOT WORKING  | ||||
| % because can not be computed with GaussianFactor.eliminate | ||||
| %if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end | ||||
| %if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end | ||||
| if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end | ||||
| if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end | ||||
|  |  | |||
|  | @ -2,7 +2,7 @@ | |||
| % equals | ||||
| fg = createGaussianFactorGraph(); | ||||
| fg2 = createGaussianFactorGraph(); | ||||
| CHECK('equals',fg.equals(fg2)); | ||||
| CHECK('equals',fg.equals(fg2,1e-9)); | ||||
| 
 | ||||
| %----------------------------------------------------------------------- | ||||
| % error | ||||
|  |  | |||
|  | @ -15,10 +15,16 @@ extern "C" { | |||
| #include <boost/shared_ptr.hpp> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace boost; // not usual, but for consiseness of generated code
 | ||||
| using namespace boost; // not usual, but for conciseness of generated code
 | ||||
| 
 | ||||
| // start GTSAM Specifics /////////////////////////////////////////////////
 | ||||
| typedef numeric::ublas::vector<double> Vector; | ||||
| typedef numeric::ublas::matrix<double> Matrix; | ||||
| // to make keys be constructed from strings:
 | ||||
| #define GTSAM_MAGIC_KEY | ||||
| // to enable Matrix and Vector constructor for SharedGaussian:
 | ||||
| #define GTSAM_MAGIC_GAUSSIAN | ||||
| // end GTSAM Specifics /////////////////////////////////////////////////
 | ||||
| 
 | ||||
| #ifdef __LP64__ | ||||
| // 64-bit Mac
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue