Created ClusterTree (duplicate of Junction tree for now, will split later). GaussianJunctionTree is no longer a template. TestJunctionTree now tests with SymbolicFactorGraph
parent
30b377e441
commit
c3a907127f
32
.cproject
32
.cproject
|
@ -396,6 +396,14 @@
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
|
<buildTarget>check</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildTarget>check</buildTarget>
|
<buildTarget>check</buildTarget>
|
||||||
|
@ -578,6 +586,30 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
|
<buildTarget>check</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testClusterTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
|
<buildTarget>testClusterTree.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testJunctionTree.run" path="build/inference" 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="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildTarget>check</buildTarget>
|
<buildTarget>check</buildTarget>
|
||||||
|
|
|
@ -0,0 +1,159 @@
|
||||||
|
/*
|
||||||
|
* ClusterTree-inl.h
|
||||||
|
* Created on: July 13, 2010
|
||||||
|
* @author Kai Ni
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <boost/foreach.hpp>
|
||||||
|
|
||||||
|
#include "SymbolicFactorGraph.h"
|
||||||
|
#include "BayesTree-inl.h"
|
||||||
|
#include "ClusterTree.h"
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template <class FG>
|
||||||
|
bool ClusterTree<FG>::Clique::equals(const ClusterTree<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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
/**
|
||||||
|
* ClusterTree
|
||||||
|
*/
|
||||||
|
template <class FG>
|
||||||
|
void ClusterTree<FG>::Clique::print(const string& indent) const {
|
||||||
|
// FG::print(indent);
|
||||||
|
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 ClusterTree<FG>::Clique::printTree(const string& indent) const {
|
||||||
|
print(indent);
|
||||||
|
BOOST_FOREACH(const shared_ptr& child, children_)
|
||||||
|
child->printTree(indent+" ");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template <class FG>
|
||||||
|
ClusterTree<FG>::ClusterTree(FG& fg, const Ordering& ordering) {
|
||||||
|
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree
|
||||||
|
SymbolicFactorGraph sfg(fg);
|
||||||
|
SymbolicBayesNet sbn = sfg.eliminate(ordering);
|
||||||
|
BayesTree<SymbolicConditional> sbt(sbn);
|
||||||
|
|
||||||
|
// distribtue factors
|
||||||
|
root_ = distributeFactors(fg, sbt.root());
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template <class FG>
|
||||||
|
typename ClusterTree<FG>::sharedClique ClusterTree<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, BayesTree<Conditional> >
|
||||||
|
ClusterTree<FG>::eliminateOneClique(sharedClique current) {
|
||||||
|
|
||||||
|
// current->frontal_.print("current clique:");
|
||||||
|
|
||||||
|
typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
|
||||||
|
FG fg; // factor graph will be assembled from local factors and marginalized children
|
||||||
|
list<BayesTree<Conditional> > children;
|
||||||
|
fg.push_back(*current); // add the local factor graph
|
||||||
|
|
||||||
|
// BOOST_FOREACH(const typename FG::sharedFactor& factor_, fg)
|
||||||
|
// Ordering(factor_->keys()).print("local factor:");
|
||||||
|
|
||||||
|
BOOST_FOREACH(sharedClique& child, current->children_) {
|
||||||
|
// receive the factors from the child and its clique point
|
||||||
|
FG fgChild; BayesTree<Conditional> childTree;
|
||||||
|
boost::tie(fgChild, childTree) = eliminateOneClique<Conditional>(child);
|
||||||
|
|
||||||
|
// BOOST_FOREACH(const typename FG::sharedFactor& factor_, fgChild)
|
||||||
|
// Ordering(factor_->keys()).print("factor from child:");
|
||||||
|
|
||||||
|
fg.push_back(fgChild);
|
||||||
|
children.push_back(childTree);
|
||||||
|
}
|
||||||
|
|
||||||
|
// eliminate the combined factors
|
||||||
|
// warning: fg is being eliminated in-place and will contain marginal afterwards
|
||||||
|
BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
|
||||||
|
|
||||||
|
// create a new clique corresponding the combined factors
|
||||||
|
BayesTree<Conditional> bayesTree(bn, children);
|
||||||
|
|
||||||
|
return make_pair(fg, bayesTree);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template <class FG> template <class Conditional>
|
||||||
|
BayesTree<Conditional> ClusterTree<FG>::eliminate() {
|
||||||
|
pair<FG, BayesTree<Conditional> > ret = this->eliminateOneClique<Conditional>(root_);
|
||||||
|
// ret.first.print("ret.first");
|
||||||
|
if (ret.first.nrFactors() != 0)
|
||||||
|
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
|
||||||
|
return ret.second;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template <class FG>
|
||||||
|
bool ClusterTree<FG>::equals(const ClusterTree<FG>& other, double tol) const {
|
||||||
|
if (!root_ || !other.root_) return false;
|
||||||
|
return root_->equals(*other.root_);
|
||||||
|
}
|
||||||
|
|
||||||
|
} //namespace gtsam
|
|
@ -0,0 +1,103 @@
|
||||||
|
/*
|
||||||
|
* ClusterTree.h
|
||||||
|
* Created on: July 13, 2010
|
||||||
|
* @author Kai Ni
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <set>
|
||||||
|
#include <boost/shared_ptr.hpp>
|
||||||
|
#include "BayesTree.h"
|
||||||
|
#include "SymbolicConditional.h"
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template <class FG>
|
||||||
|
class ClusterTree : public Testable<ClusterTree<FG> > {
|
||||||
|
public:
|
||||||
|
// the class for subgraphs that also include the pointers to the parents and two children
|
||||||
|
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 ClusterTree<FG>;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
// empty constructor
|
||||||
|
Clique() {}
|
||||||
|
|
||||||
|
// constructor with all the information
|
||||||
|
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
|
||||||
|
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_;}
|
||||||
|
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 Clique::shared_ptr sharedClique;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// Root clique
|
||||||
|
sharedClique root_;
|
||||||
|
|
||||||
|
private:
|
||||||
|
// distribute the factors along the Bayes tree
|
||||||
|
sharedClique distributeFactors(FG& fg, const BayesTree<SymbolicConditional>::sharedClique clique);
|
||||||
|
|
||||||
|
// utility function called by eliminate
|
||||||
|
template <class Conditional>
|
||||||
|
std::pair<FG, BayesTree<Conditional> > eliminateOneClique(sharedClique fg_);
|
||||||
|
|
||||||
|
public:
|
||||||
|
// constructor
|
||||||
|
ClusterTree() {}
|
||||||
|
|
||||||
|
// constructor given a factor graph and the elimination ordering
|
||||||
|
ClusterTree(FG& fg, const Ordering& ordering);
|
||||||
|
|
||||||
|
// 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 {
|
||||||
|
cout << str << endl;
|
||||||
|
if (root_.get()) root_->printTree("");
|
||||||
|
}
|
||||||
|
|
||||||
|
/** check equality */
|
||||||
|
bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const;
|
||||||
|
|
||||||
|
}; // ClusterTree
|
||||||
|
|
||||||
|
} // namespace gtsam
|
|
@ -91,7 +91,7 @@ namespace gtsam {
|
||||||
|
|
||||||
// print the object
|
// print the object
|
||||||
void print(const std::string& str) const {
|
void print(const std::string& str) const {
|
||||||
cout << str << endl;
|
std::cout << str << std::endl;
|
||||||
if (root_.get()) root_->printTree("");
|
if (root_.get()) root_->printTree("");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -24,13 +24,13 @@ check_PROGRAMS += testSymbolicFactor testSymbolicFactorGraph testSymbolicBayesNe
|
||||||
headers += inference.h inference-inl.h
|
headers += inference.h inference-inl.h
|
||||||
headers += graph.h graph-inl.h
|
headers += graph.h graph-inl.h
|
||||||
headers += FactorGraph.h FactorGraph-inl.h
|
headers += FactorGraph.h FactorGraph-inl.h
|
||||||
|
headers += ClusterTree.h ClusterTree-inl.h
|
||||||
headers += JunctionTree.h JunctionTree-inl.h
|
headers += JunctionTree.h JunctionTree-inl.h
|
||||||
headers += BayesNet.h BayesNet-inl.h
|
headers += BayesNet.h BayesNet-inl.h
|
||||||
headers += BayesTree.h BayesTree-inl.h
|
headers += BayesTree.h BayesTree-inl.h
|
||||||
headers += ISAM.h ISAM-inl.h
|
headers += ISAM.h ISAM-inl.h
|
||||||
headers += ISAM2.h ISAM2-inl.h
|
headers += ISAM2.h ISAM2-inl.h
|
||||||
check_PROGRAMS += testFactorGraph testOrdering
|
check_PROGRAMS += testFactorGraph testClusterTree testJunctionTree testBayesTree testISAM
|
||||||
check_PROGRAMS += testBayesTree testISAM
|
|
||||||
|
|
||||||
#----------------------------------------------------------------------------------------------------
|
#----------------------------------------------------------------------------------------------------
|
||||||
# discrete
|
# discrete
|
||||||
|
|
|
@ -0,0 +1,25 @@
|
||||||
|
/**
|
||||||
|
* @file testClusterTree.cpp
|
||||||
|
* @brief Unit tests for Bayes Tree
|
||||||
|
* @author Kai Ni
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include "SymbolicFactorGraph.h"
|
||||||
|
#include "ClusterTree-inl.h"
|
||||||
|
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
typedef ClusterTree<SymbolicFactorGraph> SymbolicClusterTree;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
|
@ -0,0 +1,57 @@
|
||||||
|
/**
|
||||||
|
* @file testJunctionTree.cpp
|
||||||
|
* @brief Unit tests for Bayes Tree
|
||||||
|
* @author Kai Ni
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
|
#include <boost/assign/std/set.hpp> // for operator +=
|
||||||
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include "Ordering.h"
|
||||||
|
#include "SymbolicFactorGraph.h"
|
||||||
|
#include "JunctionTree-inl.h"
|
||||||
|
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
/**
|
||||||
|
* x1 - x2 - x3 - x4
|
||||||
|
* x3 x4
|
||||||
|
* x2 x1 : x3
|
||||||
|
*/
|
||||||
|
TEST( JunctionTree, constructor )
|
||||||
|
{
|
||||||
|
SymbolicFactorGraph fg;
|
||||||
|
fg.push_factor("x1","x2");
|
||||||
|
fg.push_factor("x2","x3");
|
||||||
|
fg.push_factor("x3","x4");
|
||||||
|
|
||||||
|
Ordering ordering; ordering += "x2","x1","x3","x4";
|
||||||
|
SymbolicJunctionTree junctionTree(fg, ordering);
|
||||||
|
|
||||||
|
Ordering frontal1; frontal1 += "x3", "x4";
|
||||||
|
Ordering frontal2; frontal2 += "x2", "x1";
|
||||||
|
Unordered sep1;
|
||||||
|
Unordered sep2; sep2 += "x3";
|
||||||
|
CHECK(assert_equal(frontal1, junctionTree.root()->frontal()));
|
||||||
|
CHECK(assert_equal(sep1, junctionTree.root()->separator()));
|
||||||
|
LONGS_EQUAL(1, junctionTree.root()->size());
|
||||||
|
CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal()));
|
||||||
|
CHECK(assert_equal(sep2, junctionTree.root()->children()[0]->separator()));
|
||||||
|
LONGS_EQUAL(2, junctionTree.root()->children()[0]->size());
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
|
@ -16,7 +16,7 @@
|
||||||
#include "FactorGraph-inl.h"
|
#include "FactorGraph-inl.h"
|
||||||
#include "inference-inl.h"
|
#include "inference-inl.h"
|
||||||
#include "iterative.h"
|
#include "iterative.h"
|
||||||
#include "GaussianJunctionTree-inl.h"
|
#include "GaussianJunctionTree.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
@ -121,8 +121,8 @@ set<Symbol> GaussianFactorGraph::find_separator(const Symbol& key) const
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
GaussianConditional::shared_ptr
|
GaussianConditional::shared_ptr
|
||||||
GaussianFactorGraph::eliminateOne(const Symbol& key, bool old) {
|
GaussianFactorGraph::eliminateOne(const Symbol& key, bool enableJoinFactor) {
|
||||||
if (old)
|
if (enableJoinFactor)
|
||||||
return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*this, key);
|
return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*this, key);
|
||||||
else
|
else
|
||||||
return eliminateOneMatrixJoin(key);
|
return eliminateOneMatrixJoin(key);
|
||||||
|
@ -241,11 +241,11 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
GaussianBayesNet
|
GaussianBayesNet
|
||||||
GaussianFactorGraph::eliminate(const Ordering& ordering, bool old)
|
GaussianFactorGraph::eliminate(const Ordering& ordering, bool enableJoinFactor)
|
||||||
{
|
{
|
||||||
GaussianBayesNet chordalBayesNet; // empty
|
GaussianBayesNet chordalBayesNet; // empty
|
||||||
BOOST_FOREACH(const Symbol& key, ordering) {
|
BOOST_FOREACH(const Symbol& key, ordering) {
|
||||||
GaussianConditional::shared_ptr cg = eliminateOne(key, old);
|
GaussianConditional::shared_ptr cg = eliminateOne(key, enableJoinFactor);
|
||||||
chordalBayesNet.push_back(cg);
|
chordalBayesNet.push_back(cg);
|
||||||
}
|
}
|
||||||
return chordalBayesNet;
|
return chordalBayesNet;
|
||||||
|
@ -299,7 +299,7 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
|
VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
|
||||||
{
|
{
|
||||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(*this, ordering);
|
GaussianJunctionTree junctionTree(*this, ordering);
|
||||||
|
|
||||||
return junctionTree.optimize();
|
return junctionTree.optimize();
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,13 +1,11 @@
|
||||||
/*
|
/*
|
||||||
* GaussianJunctionTree-inl.h
|
* GaussianJunctionTree.cpp
|
||||||
*
|
|
||||||
* Created on: Jul 12, 2010
|
* Created on: Jul 12, 2010
|
||||||
* Author: nikai
|
* @author Kai Ni
|
||||||
* Description: the Gaussian junction tree
|
* @author Frank Dellaert
|
||||||
|
* @brief: the Gaussian junction tree
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
|
|
||||||
#include "JunctionTree-inl.h"
|
#include "JunctionTree-inl.h"
|
||||||
|
@ -15,33 +13,36 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
// explicit template instantiation
|
||||||
|
template class JunctionTree<GaussianFactorGraph>;
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/**
|
/**
|
||||||
* GaussianJunctionTree
|
* GaussianJunctionTree
|
||||||
*/
|
*/
|
||||||
template <class FG>
|
void GaussianJunctionTree::btreeBackSubstitue(
|
||||||
void GaussianJunctionTree<FG>::btreeBackSubstitue(typename BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config) {
|
BayesTree<GaussianConditional>::sharedClique current,
|
||||||
|
VectorConfig& config) {
|
||||||
// solve the bayes net in the current node
|
// solve the bayes net in the current node
|
||||||
typename BayesNet<GaussianConditional>::const_reverse_iterator it = current->rbegin();
|
BayesNet<GaussianConditional>::const_reverse_iterator it = current->rbegin();
|
||||||
for (; it!=current->rend(); it++) {
|
for (; it!=current->rend(); it++) {
|
||||||
Vector x = (*it)->solve(config); // Solve for that variable
|
Vector x = (*it)->solve(config); // Solve for that variable
|
||||||
config.insert((*it)->key(),x); // store result in partial solution
|
config.insert((*it)->key(),x); // store result in partial solution
|
||||||
}
|
}
|
||||||
|
|
||||||
// solve the bayes nets in the child nodes
|
// solve the bayes nets in the child nodes
|
||||||
typedef typename BayesTree<GaussianConditional>::sharedClique sharedBayesClique;
|
typedef BayesTree<GaussianConditional>::sharedClique sharedBayesClique;
|
||||||
BOOST_FOREACH(sharedBayesClique child, current->children_) {
|
BOOST_FOREACH(sharedBayesClique child, current->children_) {
|
||||||
btreeBackSubstitue(child, config);
|
btreeBackSubstitue(child, config);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template <class FG>
|
VectorConfig GaussianJunctionTree::optimize() {
|
||||||
VectorConfig GaussianJunctionTree<FG>::optimize() {
|
|
||||||
// eliminate from leaves to the root
|
// eliminate from leaves to the root
|
||||||
typedef JunctionTree<FG> Base;
|
typedef JunctionTree<GaussianFactorGraph> Base;
|
||||||
BayesTree<GaussianConditional> bayesTree;
|
BayesTree<GaussianConditional> bayesTree;
|
||||||
this->eliminate<GaussianConditional>();
|
this->eliminate<GaussianConditional>();
|
||||||
|
|
|
@ -1,9 +1,9 @@
|
||||||
/*
|
/*
|
||||||
* GaussianJunctionTree.h
|
* GaussianJunctionTree.h
|
||||||
*
|
|
||||||
* Created on: Jul 12, 2010
|
* Created on: Jul 12, 2010
|
||||||
* Author: nikai
|
* @author Kai Ni
|
||||||
* Description: the Gaussian junction tree
|
* @author Frank Dellaert
|
||||||
|
* @brief: the Gaussian junction tree
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
@ -18,22 +18,21 @@ namespace gtsam {
|
||||||
/**
|
/**
|
||||||
* GaussianJunctionTree that does the optimization
|
* GaussianJunctionTree that does the optimization
|
||||||
*/
|
*/
|
||||||
template <class FG>
|
class GaussianJunctionTree: public JunctionTree<GaussianFactorGraph> {
|
||||||
class GaussianJunctionTree: public JunctionTree<FG> {
|
|
||||||
public:
|
public:
|
||||||
typedef JunctionTree<FG> Base;
|
typedef JunctionTree<GaussianFactorGraph> Base;
|
||||||
typedef typename JunctionTree<FG>::sharedClique sharedClique;
|
typedef Base::sharedClique sharedClique;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// back-substitute in topological sort order (parents first)
|
// back-substitute in topological sort order (parents first)
|
||||||
void btreeBackSubstitue(typename BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config);
|
void btreeBackSubstitue(BayesTree<GaussianConditional>::sharedClique current, VectorConfig& config);
|
||||||
|
|
||||||
public :
|
public :
|
||||||
|
|
||||||
GaussianJunctionTree() : Base() {}
|
GaussianJunctionTree() : Base() {}
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
GaussianJunctionTree(FG& fg, const Ordering& ordering) : Base(fg, ordering) {}
|
GaussianJunctionTree(GaussianFactorGraph& fg, const Ordering& ordering) : Base(fg, ordering) {}
|
||||||
|
|
||||||
// optimize the linear graph
|
// optimize the linear graph
|
||||||
VectorConfig optimize();
|
VectorConfig optimize();
|
||||||
|
|
|
@ -19,7 +19,7 @@ check_PROGRAMS += testVectorMap testVectorBTree
|
||||||
# Gaussian Factor Graphs
|
# Gaussian Factor Graphs
|
||||||
headers += GaussianFactorSet.h Factorization.h
|
headers += GaussianFactorSet.h Factorization.h
|
||||||
sources += GaussianFactor.cpp GaussianFactorGraph.cpp
|
sources += GaussianFactor.cpp GaussianFactorGraph.cpp
|
||||||
headers += GaussianJunctionTree.h GaussianJunctionTree-inl.h
|
sources += GaussianJunctionTree.cpp
|
||||||
sources += GaussianConditional.cpp GaussianBayesNet.cpp
|
sources += GaussianConditional.cpp GaussianBayesNet.cpp
|
||||||
sources += GaussianISAM.cpp
|
sources += GaussianISAM.cpp
|
||||||
check_PROGRAMS += testGaussianFactor testGaussianJunctionTree testGaussianConditional
|
check_PROGRAMS += testGaussianFactor testGaussianJunctionTree testGaussianConditional
|
||||||
|
|
|
@ -1,9 +1,8 @@
|
||||||
/*
|
/*
|
||||||
* testJunctionTree.cpp
|
* testGaussianJunctionTree.cpp
|
||||||
*
|
*
|
||||||
* Created on: Jul 8, 2010
|
* Created on: Jul 8, 2010
|
||||||
* Author: nikai
|
* @author Kai Ni
|
||||||
* Description:
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
@ -16,45 +15,12 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include "GaussianJunctionTree-inl.h"
|
#include "Ordering.h"
|
||||||
|
#include "GaussianJunctionTree.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* x1 - x2 - x3 - x4
|
|
||||||
* x3 x4
|
|
||||||
* x2 x1 : x3
|
|
||||||
*/
|
|
||||||
TEST( GaussianFactorGraph, constructor )
|
|
||||||
{
|
|
||||||
typedef GaussianFactorGraph::sharedFactor Factor;
|
|
||||||
SharedDiagonal model(Vector_(1, 0.2));
|
|
||||||
Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model));
|
|
||||||
Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model));
|
|
||||||
Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.), model));
|
|
||||||
|
|
||||||
GaussianFactorGraph fg;
|
|
||||||
fg.push_back(factor1);
|
|
||||||
fg.push_back(factor2);
|
|
||||||
fg.push_back(factor3);
|
|
||||||
|
|
||||||
Ordering ordering; ordering += "x2","x1","x3","x4";
|
|
||||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
|
||||||
|
|
||||||
Ordering frontal1; frontal1 += "x3", "x4";
|
|
||||||
Ordering frontal2; frontal2 += "x2", "x1";
|
|
||||||
Unordered sep1;
|
|
||||||
Unordered sep2; sep2 += "x3";
|
|
||||||
CHECK(assert_equal(frontal1, junctionTree.root()->frontal()));
|
|
||||||
CHECK(assert_equal(sep1, junctionTree.root()->separator()));
|
|
||||||
LONGS_EQUAL(1, junctionTree.root()->size());
|
|
||||||
CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal()));
|
|
||||||
CHECK(assert_equal(sep2, junctionTree.root()->children()[0]->separator()));
|
|
||||||
LONGS_EQUAL(2, junctionTree.root()->children()[0]->size());
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/**
|
/**
|
||||||
* x1 - x2 - x3 - x4
|
* x1 - x2 - x3 - x4
|
||||||
|
@ -69,7 +35,7 @@ TEST( GaussianFactorGraph, constructor )
|
||||||
*
|
*
|
||||||
* 1 0 0 1
|
* 1 0 0 1
|
||||||
*/
|
*/
|
||||||
TEST( GaussianFactorGraph, eliminate )
|
TEST( GaussianJunctionTree, eliminate )
|
||||||
{
|
{
|
||||||
typedef GaussianFactorGraph::sharedFactor Factor;
|
typedef GaussianFactorGraph::sharedFactor Factor;
|
||||||
SharedDiagonal model(Vector_(1, 0.5));
|
SharedDiagonal model(Vector_(1, 0.5));
|
||||||
|
@ -85,7 +51,7 @@ TEST( GaussianFactorGraph, eliminate )
|
||||||
fg.push_back(factor4);
|
fg.push_back(factor4);
|
||||||
|
|
||||||
Ordering ordering; ordering += "x2","x1","x3","x4";
|
Ordering ordering; ordering += "x2","x1","x3","x4";
|
||||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
GaussianJunctionTree junctionTree(fg, ordering);
|
||||||
BayesTree<GaussianConditional> bayesTree = junctionTree.eliminate<GaussianConditional>();
|
BayesTree<GaussianConditional> bayesTree = junctionTree.eliminate<GaussianConditional>();
|
||||||
|
|
||||||
typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
|
typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
/*
|
/*
|
||||||
* testJunctionTree.cpp
|
* testGaussianJunctionTree.cpp
|
||||||
*
|
*
|
||||||
* Created on: Jul 8, 2010
|
* Created on: Jul 8, 2010
|
||||||
* Author: nikai
|
* Author: nikai
|
||||||
|
@ -16,8 +16,9 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include "Ordering.h"
|
||||||
|
#include "GaussianJunctionTree.h"
|
||||||
#include "smallExample.h"
|
#include "smallExample.h"
|
||||||
#include "GaussianJunctionTree-inl.h"
|
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
@ -30,7 +31,7 @@ using namespace example;
|
||||||
C3 x1 : x2
|
C3 x1 : x2
|
||||||
C4 x7 : x6
|
C4 x7 : x6
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( GaussianFactorGraph, constructor2 )
|
TEST( GaussianJunctionTree, constructor2 )
|
||||||
{
|
{
|
||||||
// create a graph
|
// create a graph
|
||||||
GaussianFactorGraph fg = createSmoother(7);
|
GaussianFactorGraph fg = createSmoother(7);
|
||||||
|
@ -38,7 +39,7 @@ TEST( GaussianFactorGraph, constructor2 )
|
||||||
// create an ordering
|
// create an ordering
|
||||||
Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
|
Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||||
|
|
||||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
GaussianJunctionTree junctionTree(fg, ordering);
|
||||||
Ordering frontal1; frontal1 += "x5", "x6", "x4";
|
Ordering frontal1; frontal1 += "x5", "x6", "x4";
|
||||||
Ordering frontal2; frontal2 += "x3", "x2";
|
Ordering frontal2; frontal2 += "x3", "x2";
|
||||||
Ordering frontal3; frontal3 += "x1";
|
Ordering frontal3; frontal3 += "x1";
|
||||||
|
@ -62,7 +63,7 @@ TEST( GaussianFactorGraph, constructor2 )
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* *
|
/* ************************************************************************* *
|
||||||
TEST( GaussianFactorGraph, optimizeMultiFrontal )
|
TEST( GaussianJunctionTree, optimizeMultiFrontal )
|
||||||
{
|
{
|
||||||
// create a graph
|
// create a graph
|
||||||
GaussianFactorGraph fg = createSmoother(7);
|
GaussianFactorGraph fg = createSmoother(7);
|
||||||
|
|
Loading…
Reference in New Issue