diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 9935776a5..4df234004 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -36,19 +36,20 @@ namespace gtsam { /* ************************************************************************* */ template BayesTreeCliqueData BayesTree::getCliqueData() const { - BayesTreeCliqueData data; - for(const sharedClique& root: roots_) - getCliqueData(data, root); - return data; + BayesTreeCliqueData stats; + for (const sharedClique& root : roots_) getCliqueData(root, &stats); + return stats; } /* ************************************************************************* */ - template - void BayesTree::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const { - data.conditionalSizes.push_back(clique->conditional()->nrFrontals()); - data.separatorSizes.push_back(clique->conditional()->nrParents()); - for(sharedClique c: clique->children) { - getCliqueData(data, c); + template + void BayesTree::getCliqueData(sharedClique clique, + BayesTreeCliqueData* stats) const { + const auto conditional = clique->conditional(); + stats->conditionalSizes.push_back(conditional->nrFrontals()); + stats->separatorSizes.push_back(conditional->nrParents()); + for (sharedClique c : clique->children) { + getCliqueData(c, stats); } } @@ -133,34 +134,26 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: Clean up namespace { - template - int _pushClique(FactorGraph& fg, const boost::shared_ptr& clique) { - fg.push_back(clique->conditional_); + template + struct _pushCliqueFunctor { + _pushCliqueFunctor(FactorGraph* graph_) : graph(graph_) {} + FactorGraph* graph; + int operator()(const boost::shared_ptr& clique, int dummy) { + graph->push_back(clique->conditional_); return 0; } - - template - struct _pushCliqueFunctor { - _pushCliqueFunctor(FactorGraph& graph_) : graph(graph_) {} - FactorGraph& graph; - int operator()(const boost::shared_ptr& clique, int dummy) { - graph.push_back(clique->conditional_); - return 0; - } - }; - } + }; + } // namespace /* ************************************************************************* */ - template - void BayesTree::addFactorsToGraph(FactorGraph& graph) const - { + template + void BayesTree::addFactorsToGraph( + FactorGraph* graph) const { // Traverse the BayesTree and add all conditionals to this graph - int data = 0; // Unused - _pushCliqueFunctor functor(graph); - treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works? -// treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique, boost::ref(graph), _1)); + int data = 0; // Unused + _pushCliqueFunctor functor(graph); + treeTraversal::DepthFirstForest(*this, data, functor); } /* ************************************************************************* */ @@ -434,50 +427,51 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) - { + template + void BayesTree::removePath(sharedClique clique, BayesNetType* bn, + Cliques* orphans) { // base case is NULL, if so we do nothing and return empties above if (clique) { - // remove the clique from orphans in case it has been added earlier - orphans.remove(clique); + orphans->remove(clique); // remove me this->removeClique(clique); // remove path above me - this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); + this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, + orphans); - // add children to list of orphans (splice also removed them from clique->children_) - orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end()); + // add children to list of orphans (splice also removed them from + // clique->children_) + orphans->insert(orphans->begin(), clique->children.begin(), + clique->children.end()); clique->children.clear(); - bn.push_back(clique->conditional_); - + bn->push_back(clique->conditional_); } } - /* ************************************************************************* */ - template - void BayesTree::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) - { + /* ************************************************************************* + */ + template + void BayesTree::removeTop(const KeyVector& keys, BayesNetType* bn, + Cliques* orphans) { + gttic(removetop); // process each key of the new factor - for(const Key& j: keys) - { + for (const Key& j : keys) { // get the clique - // TODO: Nodes will be searched again in removeClique + // TODO(frank): Nodes will be searched again in removeClique typename Nodes::const_iterator node = nodes_.find(j); - if(node != nodes_.end()) { + if (node != nodes_.end()) { // remove path from clique to root this->removePath(node->second, bn, orphans); } } // Delete cachedShortcuts for each orphan subtree - //TODO: Consider Improving - for(sharedClique& orphan: orphans) - orphan->deleteCachedShortcuts(); + // TODO(frank): Consider Improving + for (sharedClique& orphan : *orphans) orphan->deleteCachedShortcuts(); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 892ac5f31..1b9ddf7ec 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -208,13 +208,13 @@ namespace gtsam { * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. */ - void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); + void removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans); /** * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ - void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans); + void removeTop(const KeyVector& keys, BayesNetType* bn, Cliques* orphans); /** * Remove the requested subtree. */ @@ -229,7 +229,7 @@ namespace gtsam { void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); /** Add all cliques in this BayesTree to the specified factor graph */ - void addFactorsToGraph(FactorGraph& graph) const; + void addFactorsToGraph(FactorGraph* graph) const; protected: @@ -238,7 +238,7 @@ namespace gtsam { int parentnum = 0) const; /** Gather data on a single clique */ - void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const; + void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const; /** remove a clique: warning, can result in a forest */ void removeClique(sharedClique clique); @@ -249,7 +249,26 @@ namespace gtsam { // Friend JunctionTree because it directly fills roots and nodes index. template friend class EliminatableClusterTree; - private: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + public: + /// @name Deprecated + /// @{ + void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) { + removePath(clique, &bn, &orphans); + } + void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) { + removeTop(keys, &bn, &orphans); + } + void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const { + getCliqueData(clique, &stats); + } + void addFactorsToGraph(FactorGraph& graph) const{ + addFactorsToGraph(& graph); + } + /// @} +#endif + + private: /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0959989f9..600e3f9ed 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -270,7 +270,7 @@ class FactorGraph { typename std::enable_if< std::is_base_of::value>::type push_back(const BayesTree& bayesTree) { - bayesTree.addFactorsToGraph(*this); + bayesTree.addFactorsToGraph(this); } /** diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 8ed41826e..1c183b70f 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -24,14 +24,14 @@ namespace gtsam { /* ************************************************************************* */ template -void ISAM::update_internal(const FactorGraphType& newFactors, - Cliques& orphans, const Eliminate& function) { +void ISAM::updateInternal(const FactorGraphType& newFactors, + Cliques* orphans, const Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); if (!this->empty()) { KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end()); - this->removeTop(keyVector, bn, orphans); + this->removeTop(keyVector, &bn, orphans); } // Add the removed top and the new factors @@ -40,7 +40,7 @@ void ISAM::update_internal(const FactorGraphType& newFactors, factors += newFactors; // Add the orphaned subtrees - for (const sharedClique& orphan : orphans) + for (const sharedClique& orphan : *orphans) factors += boost::make_shared >(orphan); // Get an ordering where the new keys are eliminated last @@ -62,7 +62,7 @@ template void ISAM::update(const FactorGraphType& newFactors, const Eliminate& function) { Cliques orphans; - this->update_internal(newFactors, orphans, function); + this->updateInternal(newFactors, &orphans, function); } } diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h index d6a40b539..fe6763a13 100644 --- a/gtsam/inference/ISAM.h +++ b/gtsam/inference/ISAM.h @@ -22,56 +22,67 @@ namespace gtsam { +/** + * A Bayes tree with an update methods that implements the iSAM algorithm. + * Given a set of new factors, it re-eliminates the invalidated part of the + * tree. \nosubgrouping + */ +template +class ISAM : public BAYESTREE { + public: + typedef BAYESTREE Base; + typedef typename Base::BayesNetType BayesNetType; + typedef typename Base::FactorGraphType FactorGraphType; + typedef typename Base::Clique Clique; + typedef typename Base::sharedClique sharedClique; + typedef typename Base::Cliques Cliques; + + private: + typedef typename Base::Eliminate Eliminate; + typedef typename Base::EliminationTraitsType EliminationTraitsType; + + public: + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + ISAM() {} + + /** Copy constructor */ + explicit ISAM(const Base& bayesTree) : Base(bayesTree) {} + + /// @} + /// @name Advanced Interface Interface + /// @{ + /** - * A Bayes tree with an update methods that implements the iSAM algorithm. - * Given a set of new factors, it re-eliminates the invalidated part of the tree. - * \nosubgrouping + * update the Bayes tree with a set of new factors, typically derived from + * measurements + * @param newFactors is a factor graph that contains the new factors + * @param function an elimination routine */ - template - class ISAM: public BAYESTREE - { - public: - - typedef BAYESTREE Base; - typedef typename Base::BayesNetType BayesNetType; - typedef typename Base::FactorGraphType FactorGraphType; - typedef typename Base::Clique Clique; - typedef typename Base::sharedClique sharedClique; - typedef typename Base::Cliques Cliques; - - private: - - typedef typename Base::Eliminate Eliminate; - typedef typename Base::EliminationTraitsType EliminationTraitsType; - - public: - - /// @name Standard Constructors - /// @{ - - /** Create an empty Bayes Tree */ - ISAM() {} - - /** Copy constructor */ - ISAM(const Base& bayesTree) : Base(bayesTree) {} - - /// @} - /// @name Advanced Interface Interface - /// @{ - - /** - * update the Bayes tree with a set of new factors, typically derived from measurements - * @param newFactors is a factor graph that contains the new factors - * @param function an elimination routine - */ - void update(const FactorGraphType& newFactors, const Eliminate& function = EliminationTraitsType::DefaultEliminate); - - /** update_internal provides access to list of orphans for drawing purposes */ - void update_internal(const FactorGraphType& newFactors, Cliques& orphans, + void update( + const FactorGraphType& newFactors, const Eliminate& function = EliminationTraitsType::DefaultEliminate); - /// @} + /** updateInternal provides access to list of orphans for drawing purposes + */ + void updateInternal( + const FactorGraphType& newFactors, Cliques* orphans, + const Eliminate& function = EliminationTraitsType::DefaultEliminate); - }; + /// @} -}/// namespace gtsam +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + void update_internal( + const FactorGraphType& newFactors, Cliques& orphans, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) { + updateInternal(newFactors, &orphans, function); + } + /// @} +#endif +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c01d1c536..c38faae11 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -241,7 +241,7 @@ boost::shared_ptr ISAM2::recalculate( Cliques orphans; GaussianBayesNet affectedBayesNet; this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()), - affectedBayesNet, orphans); + &affectedBayesNet, &orphans); gttoc(removetop); // FactorGraph factors(affectedBayesNet);