Added accessor for NonlinearISAM, comments and whitespace
							parent
							
								
									d87ffe7ece
								
							
						
					
					
						commit
						e4cda1b360
					
				|  | @ -118,15 +118,16 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::Clique::print(const string& s) const { | ||||
| 			cout << s << "Clique "; | ||||
| 			BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { cout << conditional->key() << " "; } | ||||
| 			cout << "| "; | ||||
| 			BOOST_FOREACH(const Index sep, separator_) { cout << sep << " "; } | ||||
| 			cout << "\n"; | ||||
| 			BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { | ||||
| 				conditional->print("  " + s + "conditional"); | ||||
| 			} | ||||
| 		} | ||||
| 		cout << s << "Clique "; | ||||
| 		BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) | ||||
| 			cout << conditional->key() << " "; | ||||
| 		cout << "| "; | ||||
| 		BOOST_FOREACH(const Index sep, separator_) | ||||
| 			cout << sep << " "; | ||||
| 		cout << "\n"; | ||||
| 		BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) | ||||
| 			conditional->print("  " + s + "conditional"); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class CONDITIONAL> | ||||
|  |  | |||
|  | @ -43,13 +43,14 @@ namespace gtsam { | |||
| 
 | ||||
| 	public: | ||||
| 
 | ||||
| 	  typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr; | ||||
| 	    typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr; | ||||
| 		typedef boost::shared_ptr<CONDITIONAL> sharedConditional; | ||||
| 		typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet; | ||||
| 
 | ||||
| 		/** A Clique in the tree is an incomplete Bayes net: the variables
 | ||||
| 		/**
 | ||||
| 		 * A Clique in the tree is an incomplete Bayes net: the variables | ||||
| 		 * in the Bayes net are the frontal nodes, and the variables conditioned | ||||
| 		 * on is the separator. We also have pointers up and down the tree. | ||||
| 		 * on are the separator. We also have pointers up and down the tree. | ||||
| 		 */ | ||||
| 		struct Clique: public BayesNet<CONDITIONAL> { | ||||
| 
 | ||||
|  | @ -62,7 +63,7 @@ namespace gtsam { | |||
| 			weak_ptr parent_; | ||||
| 			std::list<shared_ptr> children_; | ||||
| 			std::list<Index> separator_; /** separator keys */ | ||||
| 	    typename CONDITIONAL::Factor::shared_ptr cachedFactor_; | ||||
| 			typename CONDITIONAL::Factor::shared_ptr cachedFactor_; | ||||
| 
 | ||||
| 			friend class BayesTree<CONDITIONAL>; | ||||
| 
 | ||||
|  | @ -119,7 +120,8 @@ namespace gtsam { | |||
| 
 | ||||
| 			/** return the joint P(C1,C2), where C1==this. TODO: not a method? */ | ||||
| 			FactorGraph<typename CONDITIONAL::Factor> joint(shared_ptr C2, shared_ptr root); | ||||
| 		}; | ||||
| 
 | ||||
| 		}; // \struct Clique
 | ||||
| 
 | ||||
| 		// typedef for shared pointers to cliques
 | ||||
| 		typedef boost::shared_ptr<Clique> sharedClique; | ||||
|  | @ -174,7 +176,8 @@ namespace gtsam { | |||
| 		sharedClique addClique(const sharedConditional& conditional, | ||||
| 				std::list<sharedClique>& child_cliques); | ||||
| 
 | ||||
| 		/** Add a conditional to the front of a clique, i.e. a conditional whose
 | ||||
| 		/**
 | ||||
| 		 * Add a conditional to the front of a clique, i.e. a conditional whose | ||||
| 		 * parents are already in the clique or its separators.  This function does | ||||
| 		 * not check for this condition, it just updates the data structures. | ||||
| 		 */ | ||||
|  | @ -198,8 +201,7 @@ namespace gtsam { | |||
| 		BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees); | ||||
| 
 | ||||
| 		/** Destructor */ | ||||
| 		virtual ~BayesTree() { | ||||
| 		} | ||||
| 		virtual ~BayesTree() {} | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Constructing Bayes trees | ||||
|  | @ -208,7 +210,8 @@ namespace gtsam { | |||
| 		/** Insert a new conditional */ | ||||
| 		void insert(const sharedConditional& conditional); | ||||
| 
 | ||||
| 		/** Insert a new clique corresponding to the given Bayes net.
 | ||||
| 		/**
 | ||||
| 		 * Insert a new clique corresponding to the given Bayes net. | ||||
| 		 * It is the caller's responsibility to decide whether the given Bayes net is a valid clique, | ||||
| 		 * i.e. all the variables (frontal and separator) are connected | ||||
| 		 */ | ||||
|  |  | |||
|  | @ -45,11 +45,10 @@ void NonlinearISAM<Values>::update(const Factors& newFactors, const Values& init | |||
|     linPoint_.insert(initialValues); | ||||
| 
 | ||||
|     // Augment ordering
 | ||||
|     BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) { | ||||
|       BOOST_FOREACH(const Symbol& key, factor->keys()) { | ||||
|         ordering_.tryInsert(key, ordering_.nVars()); | ||||
|       } | ||||
|     } | ||||
|     // FIXME: should just loop over new values
 | ||||
|     BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) | ||||
|       BOOST_FOREACH(const Symbol& key, factor->keys()) | ||||
|         ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
 | ||||
| 
 | ||||
|     boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors( | ||||
|         newFactors.linearize(linPoint_, ordering_)->template dynamicCastFactors<GaussianFactorGraph>()); | ||||
|  |  | |||
|  | @ -53,11 +53,14 @@ protected: | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 	/** default constructor will disable periodic reordering */ | ||||
| 	NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {} | ||||
| 
 | ||||
| 	/** Periodically reorder and relinearize */ | ||||
| 	NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {} | ||||
| 	/**
 | ||||
| 	 * Periodically reorder and relinearize | ||||
| 	 * @param reorderInterval is the number of updates between reorderings, | ||||
| 	 * 	0 never reorders (and is dangerous for memory consumption) | ||||
| 	 *  1 (default) reorders every time, in worse case is batch every update | ||||
| 	 *  typical values are 50 or 100 | ||||
| 	 */ | ||||
| 	NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {} | ||||
| 
 | ||||
| 	/** Add new factors along with their initial linearization points */ | ||||
| 	void update(const Factors& newFactors, const Values& initialValues); | ||||
|  | @ -70,6 +73,9 @@ public: | |||
| 
 | ||||
| 	// access
 | ||||
| 
 | ||||
| 	/** access the underlying bayes tree */ | ||||
| 	const GaussianISAM& bayesTree() const { return isam_; } | ||||
| 
 | ||||
| 	/** Return the current linearization point */ | ||||
| 	const Values& getLinearizationPoint() const { return linPoint_; } | ||||
| 
 | ||||
|  |  | |||
|  | @ -33,6 +33,9 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * An ordering is a map from symbols (non-typed keys) to integer indices | ||||
|  */ | ||||
| class Ordering : Testable<Ordering> { | ||||
| protected: | ||||
|   typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator; | ||||
|  | @ -62,6 +65,8 @@ public: | |||
|   iterator end() { return order_.end(); } | ||||
|   const_iterator end() const { return order_.end(); } | ||||
| 
 | ||||
|   // access to integer indices
 | ||||
| 
 | ||||
|   Index& at(const Symbol& key) { return operator[](key); } | ||||
|   Index at(const Symbol& key) const { return operator[](key); } | ||||
|   bool tryAt(const Symbol& key, Index& index) const { | ||||
|  | @ -77,18 +82,29 @@ public: | |||
|   Index operator[](const Symbol& key) const { | ||||
|     const_iterator i=order_.find(key); assert(i != order_.end()); return i->second; } | ||||
| 
 | ||||
|   iterator insert(const value_type& key_order) { | ||||
|     std::pair<iterator,bool> it_ok(tryInsert(key_order)); | ||||
|     assert(it_ok.second); | ||||
|     return it_ok.first; } | ||||
|   iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); } | ||||
|   // adding symbols
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(), | ||||
|    * i.e., returns a pair of iterator and success (false if already present) | ||||
|    */ | ||||
|   std::pair<iterator,bool> tryInsert(const value_type& key_order) { | ||||
|     std::pair<iterator,bool> it_ok(order_.insert(key_order)); | ||||
|     if(it_ok.second == true && key_order.second+1 > nVars_) | ||||
|       nVars_ = key_order.second+1; | ||||
|     return it_ok; } | ||||
|   	std::pair<iterator,bool> it_ok(order_.insert(key_order)); | ||||
|   	if(it_ok.second == true && key_order.second+1 > nVars_) | ||||
|   		nVars_ = key_order.second+1; | ||||
|   	return it_ok; | ||||
|   } | ||||
|   std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); } | ||||
| 
 | ||||
|   /** Try insert, but will fail if the key is already present */ | ||||
|   iterator insert(const value_type& key_order) { | ||||
|   	std::pair<iterator,bool> it_ok(tryInsert(key_order)); | ||||
|   	assert(it_ok.second); | ||||
|   	return it_ok.first; | ||||
|   } | ||||
|   iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); } | ||||
| 
 | ||||
| 
 | ||||
|   bool exists(const Symbol& key) const { return order_.count(key); } | ||||
| 
 | ||||
|   Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; } | ||||
|  |  | |||
|  | @ -19,7 +19,7 @@ const double tol=1e-5; | |||
| /* ************************************************************************* */ | ||||
| TEST(testNonlinearISAM, markov_chain ) { | ||||
| 	int reorder_interval = 2; | ||||
| 	PlanarISAM isam(reorder_interval); | ||||
| 	PlanarISAM isam(reorder_interval); // create an ISAM object
 | ||||
| 
 | ||||
| 	SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5)); | ||||
| 	Sampler sampler(model, 42u); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue