diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 6074b11b9..316135483 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -118,15 +118,16 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::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 diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 4ce0fd735..6f99d4470 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -43,13 +43,14 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > 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 { @@ -62,7 +63,7 @@ namespace gtsam { weak_ptr parent_; std::list children_; std::list separator_; /** separator keys */ - typename CONDITIONAL::Factor::shared_ptr cachedFactor_; + typename CONDITIONAL::Factor::shared_ptr cachedFactor_; friend class BayesTree; @@ -119,7 +120,8 @@ namespace gtsam { /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ FactorGraph joint(shared_ptr C2, shared_ptr root); - }; + + }; // \struct Clique // typedef for shared pointers to cliques typedef boost::shared_ptr sharedClique; @@ -174,7 +176,8 @@ namespace gtsam { sharedClique addClique(const sharedConditional& conditional, std::list& 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& bayesNet, std::list > 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 */ diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 607bb1803..7a26b0076 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -45,11 +45,10 @@ void NonlinearISAM::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 linearizedNewFactors( newFactors.linearize(linPoint_, ordering_)->template dynamicCastFactors()); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index fe7a2001b..4a5c7f361 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -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_; } diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index bfa14bb09..c81390aeb 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -33,6 +33,9 @@ namespace gtsam { +/** + * An ordering is a map from symbols (non-typed keys) to integer indices + */ class Ordering : Testable { protected: typedef boost::fast_pool_allocator > 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 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 tryInsert(const value_type& key_order) { - std::pair 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 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 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 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; } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index dcdc8dabf..ea47b0b9a 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -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);