12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #46 from borglab/fix/Bayestree_in_out 

Google-style output args
release/4.3a0
Frank Dellaert 2019-06-03 17:19:13 -04:00 committed by GitHub
parent 228ac650be a30855181a
commit 161e363190
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 136 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
gtsam/inference/BayesTree-inst.h
View File

@ -36,19 +36,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
BayesTreeCliqueData BayesTree<CLIQUE>::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 <class CLIQUE>
void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
data.separatorSizes.push_back(clique->conditional()->nrParents());
void BayesTree<CLIQUE>::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(data, c);
getCliqueData(c, stats);
}
}
@ -133,34 +134,26 @@ namespace gtsam {
}
/* ************************************************************************* */
// TODO: Clean up
namespace {
template<class FACTOR, class CLIQUE>
int _pushClique(FactorGraph<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) {
fg.push_back(clique->conditional_);
return 0;
}
template <class FACTOR, class CLIQUE>
struct _pushCliqueFunctor {
_pushCliqueFunctor(FactorGraph<FACTOR>& graph_) : graph(graph_) {}
FactorGraph<FACTOR>& graph;
_pushCliqueFunctor(FactorGraph<FACTOR>* graph_) : graph(graph_) {}
FactorGraph<FACTOR>* graph;
int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
graph.push_back(clique->conditional_);
graph->push_back(clique->conditional_);
return 0;
}
};
}
} // namespace
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::addFactorsToGraph(FactorGraph<FactorType>& graph) const
{
void BayesTree<CLIQUE>::addFactorsToGraph(
FactorGraph<FactorType>* graph) const {
// Traverse the BayesTree and add all conditionals to this graph
int data = 0; // Unused
_pushCliqueFunctor<FactorType, CLIQUE> functor(graph);
treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works?
// treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique<FactorType,CLIQUE>, boost::ref(graph), _1));
treeTraversal::DepthFirstForest(*this, data, functor);
}
/* ************************************************************************* */
@ -435,38 +428,40 @@ namespace gtsam {
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
{
void BayesTree<CLIQUE>::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 <class CLIQUE>
void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans)
{
void BayesTree<CLIQUE>::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()) {
// remove path from clique to root
@ -475,9 +470,8 @@ namespace gtsam {
}
// 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();
}
/* ************************************************************************* */

27
gtsam/inference/BayesTree.h
View File

@ -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<FactorType>& graph) const;
void addFactorsToGraph(FactorGraph<FactorType>* 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,6 +249,25 @@ namespace gtsam {
// Friend JunctionTree because it directly fills roots and nodes index.
template<class BAYESRTEE, class GRAPH> friend class EliminatableClusterTree;
#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<FactorType>& graph) const{
addFactorsToGraph(& graph);
}
/// @}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;

2
gtsam/inference/FactorGraph.h
View File

@ -270,7 +270,7 @@ class FactorGraph {
typename std::enable_if<
std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
push_back(const BayesTree<CLIQUE>& bayesTree) {
bayesTree.addFactorsToGraph(*this);
bayesTree.addFactorsToGraph(this);
}
/**

10
gtsam/inference/ISAM-inst.h
View File

@ -24,14 +24,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
Cliques& orphans, const Eliminate& function) {
void ISAM<BAYESTREE>::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<BAYESTREE>::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<BayesTreeOrphanWrapper<Clique> >(orphan);
// Get an ordering where the new keys are eliminated last
@ -62,7 +62,7 @@ template<class BAYESTREE>
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
const Eliminate& function) {
Cliques orphans;
this->update_internal(newFactors, orphans, function);
this->updateInternal(newFactors, &orphans, function);
}
}

37
gtsam/inference/ISAM.h
View File

@ -24,14 +24,12 @@ 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
* Given a set of new factors, it re-eliminates the invalidated part of the
* tree. \nosubgrouping
*/
template <class BAYESTREE>
class ISAM: public BAYESTREE
{
class ISAM : public BAYESTREE {
public:
typedef BAYESTREE Base;
typedef typename Base::BayesNetType BayesNetType;
typedef typename Base::FactorGraphType FactorGraphType;
@ -40,12 +38,10 @@ namespace gtsam {
typedef typename Base::Cliques Cliques;
private:
typedef typename Base::Eliminate Eliminate;
typedef typename Base::EliminationTraitsType EliminationTraitsType;
public:
/// @name Standard Constructors
/// @{
@ -53,25 +49,40 @@ namespace gtsam {
ISAM() {}
/** Copy constructor */
ISAM(const Base& bayesTree) : Base(bayesTree) {}
explicit ISAM(const Base& bayesTree) : Base(bayesTree) {}
/// @}
/// @name Advanced Interface Interface
/// @{
/**
* update the Bayes tree with a set of new factors, typically derived from measurements
* 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);
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,
/** updateInternal provides access to list of orphans for drawing purposes
*/
void updateInternal(
const FactorGraphType& newFactors, Cliques* orphans,
const Eliminate& function = EliminationTraitsType::DefaultEliminate);
/// @}
#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
} // namespace gtsam

2
gtsam/nonlinear/ISAM2.cpp
View File

@ -241,7 +241,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
Cliques orphans;
GaussianBayesNet affectedBayesNet;
this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()),
affectedBayesNet, orphans);
&affectedBayesNet, &orphans);
gttoc(removetop);
// FactorGraph<GaussianFactor> factors(affectedBayesNet);