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

distribute the factors along the junction tree

release/4.3a0
Kai Ni 2010-07-09 07:31:15 +00:00
parent 59cee45022
commit 6248db54dd
13 changed files with 918 additions and 783 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
.cproject
View File

@ -12,7 +12,8 @@
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.MakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -301,7 +302,6 @@
<buildTargets>
<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -309,7 +309,6 @@
</target>
<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -333,6 +332,7 @@
</target>
<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -492,7 +492,6 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -500,6 +499,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -507,7 +507,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -699,7 +698,6 @@
</target>
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -755,6 +753,7 @@
</target>
<target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -802,7 +801,6 @@
</target>
<target name="testErrors.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -810,7 +808,6 @@
</target>
<target name="testDSF.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -826,7 +823,6 @@
</target>
<target name="testConstraintOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstraintOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -834,6 +830,7 @@
</target>
<target name="testBTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -841,7 +838,6 @@
</target>
<target name="testSimulated2DOriented.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -849,6 +845,7 @@
</target>
<target name="testDSFVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -856,6 +853,7 @@
</target>
<target name="testSPQRUtil.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSPQRUtil.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -863,11 +861,20 @@
</target>
<target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testJunctionTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

7
cpp/BayesTree.h
View File

@ -67,6 +67,9 @@ namespace gtsam {
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_==NULL;}
/** return the const reference of children */
const std::list<shared_ptr>& children() const { return children_; }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
@ -230,9 +233,7 @@ namespace gtsam {
* Altering Bayes trees
*/
/**
* Remove all nodes
*/
/** Remove all nodes */
void clear();
/**

12
cpp/FactorGraph-inl.h
View File

@ -165,7 +165,7 @@ std::pair<FactorGraph<Factor>, set<Symbol> > FactorGraph<Factor>::removeSingleto
singletons.insert(new_singletons.begin(), new_singletons.end());
BOOST_FOREACH(const Symbol& singleton, new_singletons)
findAndRemoveFactors(singleton);
findAndRemoveFactors<vector<boost::shared_ptr<Factor> > >(singleton);
// exit when there are no more singletons
if (new_singletons.empty()) break;
@ -317,9 +317,8 @@ list<int> FactorGraph<Factor>::factors(const Symbol& key) const {
/* ************************************************************************* */
/** find all non-NULL factors for a variable, then set factors to NULL */
/* ************************************************************************* */
template<class Factor>
vector<boost::shared_ptr<Factor> >
FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
template<class Factor> template<class Factors>
Factors FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
// find all factor indices associated with the key
Indices::const_iterator it = indices_.find(key);
@ -329,7 +328,7 @@ FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
+ (string)key + " not found");
const list<int>& factorsAssociatedWithKey = it->second;
vector<sharedFactor> found;
Factors found;
BOOST_FOREACH(const int& i, factorsAssociatedWithKey) {
sharedFactor& fi = factors_.at(i); // throws exception !
if(fi == NULL) continue; // skip NULL factors
@ -477,7 +476,8 @@ std::pair<FactorGraph<Factor>, FactorGraph<Factor> > FactorGraph<Factor>::splitM
template<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key)
{
vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
typedef vector<boost::shared_ptr<Factor> > Factors;
Factors found = factorGraph.template findAndRemoveFactors<Factors>(key);
boost::shared_ptr<Factor> new_factor(new Factor(found));
return new_factor;
}

3
cpp/FactorGraph.h
View File

@ -123,7 +123,8 @@ namespace gtsam {
* from the factor graph
* @param key the key for the given node
*/
std::vector<sharedFactor> findAndRemoveFactors(const Symbol& key);
template<class Factors>
Factors findAndRemoveFactors(const Symbol& key);
/**
* find the minimum spanning tree using boost graph library

12
cpp/GaussianFactorGraph.cpp
View File

@ -16,6 +16,7 @@
#include "FactorGraph-inl.h"
#include "inference-inl.h"
#include "iterative.h"
#include "JunctionTree-inl.h"
using namespace std;
using namespace gtsam;
@ -207,7 +208,8 @@ std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix(
GaussianConditional::shared_ptr
GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
// find and remove all factors connected to key
vector<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
typedef vector<GaussianFactor::shared_ptr> Factors;
Factors factors = findAndRemoveFactors<Factors>(key);
// Collect all dimensions as well as the set of separator keys
set<Symbol> separator;
@ -285,6 +287,14 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
return delta;
}
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
{
GaussianJunctionTree<GaussianFactorGraph> junctionTree(*this, ordering);
return junctionTree.optimize();
}
/* ************************************************************************* */
boost::shared_ptr<GaussianBayesNet>
GaussianFactorGraph::eliminate_(const Ordering& ordering)

5
cpp/GaussianFactorGraph.h
View File

@ -154,6 +154,11 @@ namespace gtsam {
*/
VectorConfig optimize(const Ordering& ordering, bool enableJoinFactor = true);
/**
* optimize a linear factor graph with multi-frontals
*/
VectorConfig optimizeMultiFrontals(const Ordering& ordering);
/**
* shared pointer versions for MATLAB
*/

143
cpp/JunctionTree-inl.h
View File

@ -8,43 +8,113 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include "Pose2.h"
#include "BayesTree-inl.h"
#include "SymbolicFactorGraph.h"
#include "BayesTree-inl.h"
#include "JunctionTree.h"
#define DEBUG(i) \
if (verboseLevel>i) cout
namespace gtsam {
using namespace std;
/* ************************************************************************* */
template <class FG>
bool JunctionTree<FG>::Clique::equals(const JunctionTree<FG>::Clique& other) const {
if (!frontal_.equals(other.frontal_))
return false;
if (!separator_.equals(other.separator_))
return false;
if (children_.size() != other.children_.size())
return false;
typename vector<shared_ptr>::const_iterator it1 = children_.begin();
typename vector<shared_ptr>::const_iterator it2 = other.children_.begin();
for(; it1!=children_.end(); it1++, it2++)
if (!(*it1)->equals(**it2)) return false;
return true;
}
/* ************************************************************************* */
/**
* Linear JunctionTree
* JunctionTree
*/
template <class Conditional, class FG>
void JunctionTree<Conditional, FG>::SubFG::printTree(const string& indent) const {
template <class FG>
void JunctionTree<FG>::Clique::print(const string& indent) const {
// FG::print(indent);
cout << "kai1" << endl;
cout << indent;
BOOST_FOREACH(const Symbol& key, frontal_)
cout << (string)key << " ";
cout << ":";
BOOST_FOREACH(const Symbol& key, separator_)
cout << (string)key << " ";
cout << endl;
}
/* ************************************************************************* */
template <class FG>
void JunctionTree<FG>::Clique::printTree(const string& indent) const {
print(indent);
BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent+" ");
}
/* ************************************************************************* */
template <class Conditional, class FG>
template <class FG>
JunctionTree<FG>::JunctionTree(FG& fg, const Ordering& ordering) {
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree
SymbolicFactorGraph sfg(fg);
SymbolicBayesNet sbn = sfg.eliminate(ordering);
BayesTree<SymbolicConditional> sbt(sbn);
sbt.print("sbt");
// distribtue factors
root_ = distributeFactors(fg, sbt.root());
}
/* ************************************************************************* */
template <class FG>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(FG& fg,
const BayesTree<SymbolicConditional>::sharedClique bayesClique) {
// create a new clique in the junction tree
sharedClique clique(new Clique());
clique->frontal_ = bayesClique->ordering();
clique->separator_.insert(bayesClique->separator_.begin(), bayesClique->separator_.end());
// recursively call the children
BOOST_FOREACH(const BayesTree<SymbolicConditional>::sharedClique bayesChild, bayesClique->children()) {
sharedClique child = distributeFactors(fg, bayesChild);
clique->children_.push_back(child);
child->parent_ = clique;
}
// collect the factors
typedef vector<typename FG::sharedFactor> Factors;
BOOST_FOREACH(const Symbol& frontal, clique->frontal_) {
Factors factors = fg.template findAndRemoveFactors<Factors>(frontal);
BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors) {
clique->push_back(factor_);
}
}
return clique;
}
/* ************************************************************************* */
template <class FG> template <class Conditional>
pair<FG, typename BayesTree<Conditional>::sharedClique>
JunctionTree<Conditional, FG>::eliminateOneClique(sharedSubFG current, BayesTree<Conditional>& bayesTree) {
JunctionTree<FG>::eliminateOneClique(sharedClique current, BayesTree<Conditional>& bayesTree) {
typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
FG fg; // factor graph will be assembled from local factors and marginalized children
list<sharedClique> children;
list<sharedBtreeClique> children;
fg.push_back(*current); // add the local factor graph
BOOST_FOREACH(sharedSubFG& child, current->children_) {
BOOST_FOREACH(sharedClique& child, current->children_) {
// receive the factors from the child and its clique point
FG fgChild; sharedClique cliqueChild;
FG fgChild; sharedBtreeClique cliqueChild;
boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree);
if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!");
@ -58,57 +128,54 @@ namespace gtsam {
BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
// create a new clique corresponding the combined factors
sharedClique new_clique = bayesTree.insert(bn, children);
sharedBtreeClique new_clique = bayesTree.insert(bn, children);
return make_pair(fg, new_clique);
}
/* ************************************************************************* */
template <class Conditional, class FG>
BayesTree<Conditional> JunctionTree<Conditional, FG>::eliminate() {
template <class FG> template <class Conditional>
BayesTree<Conditional> JunctionTree<FG>::eliminate() {
BayesTree<Conditional> bayesTree;
eliminateOneClique(rootFG_, bayesTree);
eliminateOneClique(root_, bayesTree);
return bayesTree;
}
/* ************************************************************************* */
template <class Conditional, class FG>
void JunctionTree<Conditional, FG>::iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current) {
if (!current.get()) current = rootFG_;
// iterateBFS<SubFG>(visitor, current);
}
/* ************************************************************************* */
template <class Conditional, class FG>
void JunctionTree<Conditional, FG>::iterSubGraphsBFS(VisitorSubFG visitor) {
// iterateDFS<SubFG>(visitor, rootFG_);
template <class FG>
bool JunctionTree<FG>::equals(const JunctionTree<FG>& other, double tol) const {
if (!root_ || !other.root_) return false;
return root_->equals(*other.root_);
}
/* ************************************************************************* */
/**
* Linear JunctionTree
* GaussianJunctionTree
*/
template <class Conditional, class FG>
void LinearJunctionTree<Conditional, FG>::btreeBackSubstitue(typename BayesTree<Conditional>::sharedClique current, VectorConfig& config) {
template <class FG>
void GaussianJunctionTree<FG>::btreeBackSubstitue(typename BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config) {
// solve the bayes net in the current node
typename BayesNet<Conditional>::const_reverse_iterator it = current->rbegin();
typename BayesNet<GaussianConditional>::const_reverse_iterator it = current->rbegin();
for (; it!=current->rend(); it++) {
Vector x = (*it)->solve(config); // Solve for that variable
config.insert((*it)->key(),x); // store result in partial solution
}
// solve the bayes nets in the child nodes
BOOST_FOREACH(sharedClique child, current->children_) {
typedef typename BayesTree<GaussianConditional>::sharedClique sharedBayesClique;
BOOST_FOREACH(sharedBayesClique child, current->children_) {
btreeBackSubstitue(child, config);
}
}
/* ************************************************************************* */
template <class Conditional, class FG>
VectorConfig LinearJunctionTree<Conditional, FG>::optimize() {
template <class FG>
VectorConfig GaussianJunctionTree<FG>::optimize() {
// eliminate from leaves to the root
BayesTree<Conditional> bayesTree = JunctionTree<Conditional, FG>::eliminate();
typedef JunctionTree<FG> Base;
BayesTree<GaussianConditional> bayesTree = this->eliminate<GaussianConditional>();
// back-substitution
VectorConfig result;
btreeBackSubstitue(bayesTree.root(), result);
return result;

100
cpp/JunctionTree.h
View File

@ -18,128 +18,114 @@
namespace gtsam {
/* ************************************************************************* */
template <class Conditional, class FG>
class JunctionTree/*: public BayesTree<Conditional>*/ {
template <class FG>
class JunctionTree : public Testable<JunctionTree<FG> > {
public:
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
// the threshold for the sizes of submaps. Smaller ones will be absorbed into the separator
static const int const_minNodesPerMap_default = 10;
static const int const_minNodesPerMap_ultra = 1;
// when to stop partitioning
static const int const_numNodeStopPartition_default = 50;
static const int const_numNodeStopPartition_ultra = 3; // so that A,B,C all have one variable
// the class for subgraphs that also include the pointers to the parents and two children
class SubFG : public FG {
public:
typedef typename boost::shared_ptr<SubFG> shared_ptr;
class Clique : public FG {
private:
typedef typename boost::shared_ptr<Clique> shared_ptr;
shared_ptr parent_; // the parent subgraph node
std::vector<shared_ptr> children_; // the child cliques
Ordering frontal_; // the frontal varaibles
Unordered separator_; // the separator variables
friend class JunctionTree<Conditional, FG>;
friend class JunctionTree<FG>;
public:
std::vector<shared_ptr> children_; // the child cliques
// empty constructor
SubFG() {}
Clique() {}
// constructor with all the information
SubFG(const FG& fgLocal, const Ordering& frontal, const Unordered& separator,
Clique(const FG& fgLocal, const Ordering& frontal, const Unordered& separator,
const shared_ptr& parent)
: frontal_(frontal), separator_(separator), FG(fgLocal), parent_(parent) {}
// constructor for an empty graph
SubFG(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent)
Clique(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent)
: frontal_(frontal), separator_(separator), parent_(parent) {}
// return the members
const Ordering& frontal() const { return frontal_;}
const Unordered& separator() const { return separator_;}
std::vector<shared_ptr>& children() { return children_; } // TODO:: add const
const std::vector<shared_ptr>& children() { return children_; }
// add a child node
void addChild(const shared_ptr& child) { children_.push_back(child); }
// print the object
void print(const std::string& indent) const;
void printTree(const std::string& indent) const;
// check equality
bool equals(const Clique& other) const;
};
// typedef for shared pointers to cliques
typedef typename SubFG::shared_ptr sharedSubFG;
typedef boost::function<void (sharedSubFG)> VisitorSubFG;
typedef typename Clique::shared_ptr sharedClique;
protected:
// Root clique
sharedSubFG rootFG_;
sharedClique root_;
private:
// distribute the factors along the Bayes tree
sharedClique distributeFactors(FG& fg, const BayesTree<SymbolicConditional>::sharedClique clique);
// utility function called by eliminateBottomUp
std::pair<FG, sharedClique> eliminateOneClique(sharedSubFG fg_, BayesTree<Conditional>& bayesTree);
template <class Conditional>
std::pair<FG, typename BayesTree<Conditional>::sharedClique> eliminateOneClique(
sharedClique fg_, BayesTree<Conditional>& bayesTree);
public:
// constructor
JunctionTree() {}
JunctionTree() : verboseLevel(0) {}
// constructor given a factor graph and the elimination ordering
JunctionTree(FG& fg, const Ordering& ordering);
// return the root graph
sharedSubFG rootFG() const { return rootFG_; }
// return the root clique
sharedClique root() const { return root_; }
// eliminate the factors in the subgraphs
template <class Conditional>
BayesTree<Conditional> eliminate();
// print the object
void print(const std::string& str) const {
if (rootFG_.get()) rootFG_->printTree("");
cout << str << endl;
if (root_.get()) root_->printTree("");
}
// iterate over all the subgraphs from root to leaves in the DFS order, recursive
void iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current = sharedSubFG());
/** check equality */
bool equals(const JunctionTree<FG>& other, double tol = 1e-9) const;
// iterate over all the subgraphs from root to leaves in the BFS order, non-recursive
void iterSubGraphsBFS(VisitorSubFG visitor);
// the output level
int verboseLevel;
}; // JunctionTree
/* ************************************************************************* */
/**
* Linear JunctionTree which can do optimization
* GaussianJunctionTree that does the optimization
*/
template <class Conditional, class FG>
class LinearJunctionTree: public JunctionTree<Conditional, FG> {
template <class FG>
class GaussianJunctionTree: public JunctionTree<FG> {
public:
typedef JunctionTree<Conditional, FG> Base;
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
typedef typename JunctionTree<Conditional, FG>::sharedSubFG sharedSubFG;
typedef JunctionTree<FG> Base;
typedef typename JunctionTree<FG>::sharedClique sharedClique;
protected:
// back-substitute in topological sort order (parents first)
void btreeBackSubstitue(typename BayesTree<Conditional>::sharedClique current, VectorConfig& config);
void btreeBackSubstitue(typename BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config);
public :
LinearJunctionTree() : Base() {}
GaussianJunctionTree() : Base() {}
// constructor
LinearJunctionTree(const FG& fg, const Ordering& ordering, int numNodeStopPartition = Base::const_numNodeStopPartition_default,
int minNodesPerMap = Base::const_minNodesPerMap_default) :
Base(fg, ordering, numNodeStopPartition, minNodesPerMap) {}
GaussianJunctionTree(FG& fg, const Ordering& ordering) : Base(fg, ordering) {}
// optimize the linear graph
VectorConfig optimize();
}; // Linear JunctionTree
class SymbolicConditional;
class SymbolicFactorGraph;
/**
* recursive partitioning
*/
typedef JunctionTree<SymbolicConditional, SymbolicFactorGraph> SymbolicTSAM;
typedef JunctionTree<GaussianConditional, GaussianFactorGraph> GaussianTSAM;
} // namespace gtsam

6
cpp/Makefile.am
View File

@ -75,13 +75,15 @@ headers += BayesTree.h BayesTree-inl.h
headers += ISAM.h ISAM-inl.h GaussianISAM.h
headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h
sources += GaussianISAM.cpp GaussianISAM2.cpp
check_PROGRAMS += testGraph testFactorGraph testInference testOrdering
check_PROGRAMS += testGraph testFactorGraph testJunctionTree testInference testOrdering
check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
testGraph_SOURCES = testGraph.cpp
testGraph_LDADD = libgtsam.la
testFactorGraph_SOURCES = testFactorgraph.cpp
testInference_SOURCES = testInference.cpp
testFactorGraph_LDADD = libgtsam.la
testJunctionTree_SOURCES = testJunctionTree.cpp
testJunctionTree_LDADD = libgtsam.la
testInference_SOURCES = testInference.cpp
testInference_LDADD = libgtsam.la
testBayesTree_SOURCES = testBayesTree.cpp
testBayesTree_LDADD = libgtsam.la

2
cpp/Matrix.cpp
View File

@ -693,7 +693,7 @@ void householder(Matrix &A) {
__CLPK_integer info;
dgeqrf_(&m, &n, a, &m, tau, &work_optimal_size, &lwork, &info);
lwork = (int)work_optimal_size;
lwork = (__CLPK_integer)work_optimal_size;
double work[lwork];
dgeqrf_(&m, &n, a, &m, tau, work, &lwork, &info);
int k0 = 0;

38
cpp/testGaussianFactorGraph.cpp
View File

@ -475,38 +475,6 @@ TEST( GaussianFactorGraph, optimize )
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* *
Bayes tree for smoother with "nested dissection" ordering:
C1 x5 x6 x4
C2 x3 x2 : x4
C3 x1 : x2
C4 x7 : x6
/* ************************************************************************* */
TEST( GaussianFactorGraph, optimizeMultiFrontal )
{
// create a graph
GaussianFactorGraph fg = createSmoother(7);
// create an ordering
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
// Symbolic factorization
// GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree
SymbolicFactorGraph sfg(fg);
SymbolicBayesNet sbn = sfg.eliminate(ordering);
BayesTree<SymbolicConditional> sbt(sbn);
// // optimize the graph
// VectorConfig actual = fg.optimizeMultiFrontal(sbt);
//
// // verify
// VectorConfig expected = createCorrectDelta();
//
// CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, combine)
{
@ -584,7 +552,8 @@ TEST( GaussianFactorGraph, findAndRemoveFactors )
GaussianFactor::shared_ptr f2 = fg[2];
// call the function
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors
<vector<GaussianFactor::shared_ptr> >("x1");
// Check the factors
CHECK(f0==factors[0]);
@ -607,7 +576,8 @@ TEST( GaussianFactorGraph, findAndRemoveFactors_twice )
GaussianFactor::shared_ptr f2 = fg[2];
// call the function
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors
<vector<GaussianFactor::shared_ptr> >("x1");
// Check the factors
CHECK(f0==factors[0]);

86
cpp/testJunctionTree.cpp Normal file
View File

@ -0,0 +1,86 @@
/*
* testJunctionTree.cpp
*
* Created on: Jul 8, 2010
* Author: nikai
* Description:
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#define GTSAM_MAGIC_KEY
#include "smallExample.h"
#include "JunctionTree-inl.h"
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* *
Bayes tree for smoother with "nested dissection" ordering:
C1 x5 x6 x4
C2 x3 x2 : x4
C3 x1 : x2
C4 x7 : x6
/* ************************************************************************* */
TEST( GaussianFactorGraph, constructor )
{
// create a graph
GaussianFactorGraph fg = createSmoother(7);
// create an ordering
Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
Ordering frontal1; frontal1 += "x5", "x6", "x4";
Ordering frontal2; frontal2 += "x3", "x2";
Ordering frontal3; frontal3 += "x1";
Ordering frontal4; frontal4 += "x7";
Unordered sep1;
Unordered sep2; sep2 += "x4";
Unordered sep3; sep3 += "x2";
Unordered sep4; sep4 += "x6";
CHECK(assert_equal(frontal1, junctionTree.root()->frontal()));
CHECK(assert_equal(sep1, junctionTree.root()->separator()));
LONGS_EQUAL(5, junctionTree.root()->size());
CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal()));
CHECK(assert_equal(sep2, junctionTree.root()->children()[0]->separator()));
LONGS_EQUAL(4, junctionTree.root()->children()[0]->size());
CHECK(assert_equal(frontal3, junctionTree.root()->children()[0]->children()[0]->frontal()));
CHECK(assert_equal(sep3, junctionTree.root()->children()[0]->children()[0]->separator()));
LONGS_EQUAL(2, junctionTree.root()->children()[0]->children()[0]->size());
CHECK(assert_equal(frontal4, junctionTree.root()->children()[1]->frontal()));
CHECK(assert_equal(sep4, junctionTree.root()->children()[1]->separator()));
LONGS_EQUAL(2, junctionTree.root()->children()[1]->size());
}
/* ************************************************************************* *
TEST( GaussianFactorGraph, optimizeMultiFrontal )
{
// create a graph
GaussianFactorGraph fg = createSmoother(7);
// create an ordering
Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
// optimize the graph
LinearJunctionTree<GaussianConditional, GaussianFactorGraph> junctionTree(fg, ordering);
VectorConfig actual = junctionTree.optimize();
// verify
VectorConfig expected = createCorrectDelta();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

2
cpp/testSymbolicFactorGraph.cpp
View File

@ -46,7 +46,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors )
SymbolicFactorGraph actual(factorGraph);
SymbolicFactor::shared_ptr f1 = actual[0];
SymbolicFactor::shared_ptr f3 = actual[2];
actual.findAndRemoveFactors("x2");
actual.findAndRemoveFactors<SymbolicFactorGraph>("x2");
// construct expected graph after find_factors_and_remove
SymbolicFactorGraph expected;