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

Added FactorGraph constructor from BayesTree in unordered code. Made FactorGraph += operator templated so can += any type that works with push_back.

release/4.3a0
Richard Roberts 2013-06-24 19:30:18 +00:00
parent a6b8b3e80a
commit 99b1ab4754
4 changed files with 54 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/inference/FactorGraphUnordered-inst.h
View File

@ -16,24 +16,21 @@
* @author Frank Dellaert
* @author Alireza Fathi
* @author Michael Kaess
* @author Richard Roberts
*/
#pragma once
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/bind.hpp>
#include <stdio.h>
#include <list>
#include <sstream>
#include <stdexcept>
namespace gtsam {
@ -67,6 +64,25 @@ namespace gtsam {
return true;
}
/* ************************************************************************* */
namespace {
template<class FACTOR, class CLIQUE>
int _pushClique(FactorGraphUnordered<FACTOR>* fg, const boost::shared_ptr<CLIQUE>& clique) {
fg->push_back(clique->conditional_);
return 0;
}
}
/* ************************************************************************* */
template<class FACTOR>
template<class CLIQUE>
void FactorGraphUnordered<FACTOR>::push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree)
{
// Traverse the BayesTree and add all conditionals to this graph
int data = 0; // Unused
treeTraversal::DepthFirstForest(bayesTree, data, boost::bind(&_pushClique<FACTOR,CLIQUE>, this, _1));
}
/* ************************************************************************* */
template<class FACTOR>
size_t FactorGraphUnordered<FACTOR>::nrFactors() const {

35
gtsam/inference/FactorGraphUnordered.h
View File

@ -15,6 +15,7 @@
* @author Carlos Nieto
* @author Christian Potthast
* @author Michael Kaess
* @author Richard Roberts
*/
// \callgraph
@ -27,6 +28,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/BayesTreeUnordered.h>
namespace gtsam {
@ -39,7 +41,6 @@ namespace gtsam {
class FactorGraphUnordered {
public:
typedef FactorGraphUnordered<FACTOR> This;
typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
@ -57,7 +58,6 @@ namespace gtsam {
/** Collection of factors */
std::vector<sharedFactor> factors_;
public:
/// @name Standard Constructors
/// @{
@ -93,6 +93,8 @@ namespace gtsam {
//}
/// @}
public:
/// @name Adding Factors
/// @{
@ -107,35 +109,38 @@ namespace gtsam {
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
/** Add a factor, will be copy-constructed into a shared_ptr (use push_back to avoid the copy). */
template<class DERIVEDFACTOR>
void add(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
}
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
/** push back many factors */
void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
factors_.insert(end(), factors.begin(), factors.end()); }
/** push back many factors with an iterator */
template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor);
}
factors_.insert(end(), firstFactor, lastFactor); }
protected:
/** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
* classes in favor of a type-specialized version that calls this templated function. */
template<class CLIQUE>
void push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree);
public:
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
boost::assign::list_inserter<boost::assign_detail::call_push_back<This>, sharedFactor>
operator+=(const sharedFactor& factor)
template<class T>
boost::assign::list_inserter<boost::assign_detail::call_push_back<This>, T>
operator+=(const T& factorOrContainer)
{
return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(factor);
boost::assign_detail::call_push_back<This>(*this))(factorOrContainer);
}
/**
* @brief Add a vector of derived factors
* @param factors to add

5
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

@ -47,6 +47,11 @@ namespace gtsam {
push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2,key3,key4));
}
/* ************************************************************************* */
void SymbolicFactorGraphUnordered::push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree) {
Base::push_back_bayesTree(bayesTree);
}
// /* ************************************************************************* */
// std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
// SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const

7
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

@ -65,6 +65,10 @@ namespace gtsam {
template<typename ITERATOR>
SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
/** Constructor from a BayesTree */
SymbolicFactorGraphUnordered(const SymbolicBayesTreeUnordered& bayesTree) {
push_back_bayesTree(bayesTree); }
/// @}
/// @name Standard Interface
/// @{
@ -81,6 +85,9 @@ namespace gtsam {
/** Push back 4-way factor */
void push_factor(Key key1, Key key2, Key key3, Key key4);
/** push back a BayesTree as a collection of factors. */
void push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree);
/// @}
};