Made EliminationTree generic, with Symbolic and Gaussian unit tests
Richard Roberts
parent
fc66445be8
commit
130d9d2797
|
|
@ -71,7 +71,7 @@ int main(int argc, char** argv) {
|
|||
|
||||
/* 4.2.1 Alternatively, you can go through the process step by step
|
||||
* Choose an ordering */
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate).first;
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
|
||||
|
||||
/* 4.2.2 set up solver and optimize */
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,159 @@
|
|||
/**
|
||||
* @file EliminationTree.cpp
|
||||
* @brief
|
||||
* @author Frank Dellaert
|
||||
* @created Oct 13, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/inference/VariableSlots-inl.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#include <iostream>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
typename EliminationTree<FACTORGRAPH>::EliminationResult
|
||||
EliminationTree<FACTORGRAPH>::eliminate_() const {
|
||||
|
||||
typename FACTORGRAPH::bayesnet_type bayesNet;
|
||||
|
||||
set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > separator;
|
||||
|
||||
// Create the list of factors to be eliminated
|
||||
FACTORGRAPH factors;
|
||||
factors.reserve(this->factors_.size() + this->subTrees_.size());
|
||||
|
||||
// add all factors associated with root
|
||||
factors.push_back(this->factors_.begin(), this->factors_.end());
|
||||
|
||||
// for all children, eliminate into Bayes net and add the eliminated factors
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
|
||||
EliminationResult result = child->eliminate_();
|
||||
bayesNet.push_back(result.first);
|
||||
factors.push_back(result.second);
|
||||
}
|
||||
|
||||
// eliminate the joint factor and add the conditional to the bayes net
|
||||
typename FACTORGRAPH::sharedFactor jointFactor(FACTORGRAPH::factor_type::Combine(factors, VariableSlots(factors)));
|
||||
bayesNet.push_back(jointFactor->eliminateFirst());
|
||||
|
||||
return EliminationResult(bayesNet, jointFactor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
vector<Index> EliminationTree<FACTORGRAPH>::ComputeParents(const VariableIndex<>& structure) {
|
||||
|
||||
// Number of factors and variables
|
||||
const size_t m = structure.nFactors();
|
||||
const size_t n = structure.size();
|
||||
|
||||
static const Index none = numeric_limits<Index>::max();
|
||||
|
||||
// Allocate result parent vector and vector of last factor columns
|
||||
vector<Index> parents(n, none);
|
||||
vector<Index> prevCol(m, none);
|
||||
|
||||
// for column j \in 1 to n do
|
||||
for (Index j = 0; j < n; j++) {
|
||||
// for row i \in Struct[A*j] do
|
||||
BOOST_FOREACH(typename VariableIndex<>::mapped_factor_type i_pos, structure[j]) {
|
||||
const size_t i = i_pos.factorIndex;
|
||||
if (prevCol[i] != none) {
|
||||
Index k = prevCol[i];
|
||||
// find root r of the current tree that contains k
|
||||
Index r = k;
|
||||
while (parents[r] != none)
|
||||
r = parents[r];
|
||||
if (r != j) parents[r] = j;
|
||||
}
|
||||
prevCol[i] = j;
|
||||
}
|
||||
}
|
||||
|
||||
return parents;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
typename EliminationTree<FACTORGRAPH>::shared_ptr
|
||||
EliminationTree<FACTORGRAPH>::Create(const FACTORGRAPH& factorGraph) {
|
||||
|
||||
// Create column structure
|
||||
VariableIndex<> varIndex(factorGraph);
|
||||
|
||||
// Compute the tree structure
|
||||
vector<Index> parents(ComputeParents(varIndex));
|
||||
|
||||
// Number of variables
|
||||
const size_t n = varIndex.size();
|
||||
|
||||
static const Index none = numeric_limits<Index>::max();
|
||||
|
||||
// Create tree structure
|
||||
vector<shared_ptr> trees(n);
|
||||
for (Index k = 1; k <= n; k++) {
|
||||
Index j = n - k;
|
||||
trees[j].reset(new EliminationTree(j));
|
||||
if (parents[j] != none)
|
||||
trees[parents[j]]->add(trees[j]);
|
||||
}
|
||||
|
||||
// Hang factors in right places
|
||||
BOOST_FOREACH(const sharedFactor& factor, factorGraph) {
|
||||
Index j = factor->front();
|
||||
trees[j]->add(factor);
|
||||
}
|
||||
|
||||
// Assert that all other nodes have parents, i.e. that this is not a forest.
|
||||
#ifndef NDEBUG
|
||||
for(typename vector<shared_ptr>::const_iterator tree=trees.begin(); tree!=trees.end()-1; ++tree)
|
||||
assert((*tree) != shared_ptr());
|
||||
#endif
|
||||
|
||||
return trees.back();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
void EliminationTree<FACTORGRAPH>::print(const std::string& name) const {
|
||||
cout << name << " (" << key_ << ")" << endl;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
factor->print(name + " "); }
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
|
||||
child->print(name + " "); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& expected, double tol) const {
|
||||
if(this->key_ == expected.key_ && this->factors_ == expected.factors_
|
||||
&& this->subTrees_.size() == expected.subTrees_.size()) {
|
||||
typename SubTrees::const_iterator this_subtree = this->subTrees_.begin();
|
||||
typename SubTrees::const_iterator expected_subtree = expected.subTrees_.begin();
|
||||
while(this_subtree != this->subTrees_.end())
|
||||
if( ! (*(this_subtree++))->equals(**(expected_subtree++), tol))
|
||||
return false;
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
typename FACTORGRAPH::bayesnet_type::shared_ptr EliminationTree<FACTORGRAPH>::eliminate() const {
|
||||
|
||||
// call recursive routine
|
||||
EliminationResult result = eliminate_();
|
||||
return typename FACTORGRAPH::bayesnet_type::shared_ptr(new typename FACTORGRAPH::bayesnet_type(result.first));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/**
|
||||
* @file EliminationTree.h
|
||||
* @brief
|
||||
* @author Frank Dellaert
|
||||
* @created Oct 13, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <boost/pool/pool_alloc.hpp>
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
class EliminationTreeTester; // for unit tests, see testEliminationTree
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* An elimination tree is a tree of factors
|
||||
*/
|
||||
template<class FACTORGRAPH>
|
||||
class EliminationTree: public Testable<EliminationTree<FACTORGRAPH> > {
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<typename FACTORGRAPH::factor_type> sharedFactor;
|
||||
typedef boost::shared_ptr<EliminationTree<FACTORGRAPH> > shared_ptr;
|
||||
typedef FACTORGRAPH FactorGraph;
|
||||
|
||||
private:
|
||||
|
||||
typedef std::list<sharedFactor, boost::fast_pool_allocator<sharedFactor> > Factors;
|
||||
typedef std::list<shared_ptr, boost::fast_pool_allocator<shared_ptr> > SubTrees;
|
||||
|
||||
Index key_; /** index associated with root */
|
||||
Factors factors_; /** factors associated with root */
|
||||
SubTrees subTrees_; /** sub-trees */
|
||||
|
||||
typedef std::pair<typename FACTORGRAPH::bayesnet_type, typename FACTORGRAPH::sharedFactor> EliminationResult;
|
||||
|
||||
/** default constructor, private, as you should use Create below */
|
||||
EliminationTree(Index key = 0) : key_(key) {}
|
||||
|
||||
/** add a factor, for Create use only */
|
||||
void add(const sharedFactor& factor) { factors_.push_back(factor); }
|
||||
|
||||
/** add a subtree, for Create use only */
|
||||
void add(const shared_ptr& child) { subTrees_.push_back(child); }
|
||||
|
||||
/**
|
||||
* Static internal function to build a vector of parent pointers using the
|
||||
* algorithm of Gilbert et al., 2001, BIT.
|
||||
*/
|
||||
static std::vector<Index> ComputeParents(const VariableIndex<>& structure);
|
||||
|
||||
/**
|
||||
* Recursive routine that eliminates the factors arranged in an elimination tree
|
||||
*/
|
||||
EliminationResult eliminate_() const;
|
||||
|
||||
// Allow access to constructor and add methods for testing purposes
|
||||
friend class ::EliminationTreeTester;
|
||||
|
||||
public:
|
||||
|
||||
/** Named constructor to build the elimination tree of a factor graph */
|
||||
static shared_ptr Create(const FACTORGRAPH& factorGraph);
|
||||
|
||||
/** Print the tree to cout */
|
||||
void print(const std::string& name = "EliminationTree: ") const;
|
||||
|
||||
/** Test whether the tree is equal to another */
|
||||
bool equals(const EliminationTree& other, double tol = 1e-9) const;
|
||||
|
||||
/** Eliminate the factors to a Bayes Net */
|
||||
typename FACTORGRAPH::bayesnet_type::shared_ptr eliminate() const;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -85,6 +85,10 @@ namespace gtsam {
|
|||
/** push back many factors */
|
||||
void push_back(const FactorGraph<Factor>& factors);
|
||||
|
||||
/** push back many factors with an iterator */
|
||||
template<typename Iterator>
|
||||
void push_back(Iterator firstFactor, Iterator lastFactor);
|
||||
|
||||
/** ------------------ Querying Factor Graphs ---------------------------- */
|
||||
|
||||
/** print out graph */
|
||||
|
|
@ -249,6 +253,14 @@ namespace gtsam {
|
|||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
template<typename Iterator>
|
||||
void FactorGraph<Factor>::push_back(Iterator firstFactor, Iterator lastFactor) {
|
||||
Iterator factor = firstFactor;
|
||||
while(factor != lastFactor)
|
||||
this->push_back(*(factor++));
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ headers += VariableIndex.h
|
|||
headers += FactorGraph.h FactorGraph-inl.h
|
||||
headers += ClusterTree.h ClusterTree-inl.h
|
||||
headers += JunctionTree.h JunctionTree-inl.h
|
||||
#headers += EliminationTree.h EliminationTree-inl.h
|
||||
headers += EliminationTree.h EliminationTree-inl.h
|
||||
headers += BayesNet.h BayesNet-inl.h
|
||||
headers += BayesTree.h BayesTree-inl.h
|
||||
headers += ISAM.h ISAM-inl.h
|
||||
|
|
@ -38,7 +38,7 @@ check_PROGRAMS += tests/testFactorGraph
|
|||
check_PROGRAMS += tests/testFactorGraph
|
||||
check_PROGRAMS += tests/testBayesTree
|
||||
check_PROGRAMS += tests/testISAM
|
||||
#check_PROGRAMS += tests/testEliminationTree
|
||||
check_PROGRAMS += tests/testEliminationTree
|
||||
check_PROGRAMS += tests/testClusterTree
|
||||
check_PROGRAMS += tests/testJunctionTree
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
#include <gtsam/inference/Factor-inl.h>
|
||||
#include <gtsam/inference/inference-inl.h>
|
||||
#include <gtsam/inference/EliminationTree-inl.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -32,6 +33,7 @@ namespace gtsam {
|
|||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class FactorGraph<Factor>;
|
||||
template class BayesNet<Conditional>;
|
||||
template class EliminationTree<SymbolicFactorGraph>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<Conditional>& bayesNet) :
|
||||
|
|
|
|||
|
|
@ -80,7 +80,6 @@ public:
|
|||
const mapped_type& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
|
||||
mapped_type& operator[](Index variable) { checkVar(variable); return index_[variable]; }
|
||||
void permute(const Permutation& permutation);
|
||||
// template<class Derived> void augment(const Derived& varIndex);
|
||||
template<class FactorGraph> void augment(const FactorGraph& factorGraph);
|
||||
void rebaseFactors(ptrdiff_t baseIndexChange);
|
||||
|
||||
|
|
@ -154,21 +153,6 @@ VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
|
|||
}
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//template<class Storage>
|
||||
//template<class Derived>
|
||||
//void VariableIndex<Storage>::augment(const Derived& varIndex) {
|
||||
// nFactors_ = std::max(nFactors_, varIndex.nFactors());
|
||||
// nEntries_ = nEntries_ + varIndex.nEntries();
|
||||
// Index originalSize = index_.size();
|
||||
// index_.container().resize(std::max(index_.size(), varIndex.size()));
|
||||
// index_.permutation().resize(index_.container().size());
|
||||
// for(Index var=originalSize; var<index_.permutation().size(); ++var)
|
||||
// index_.permutation()[var] = var;
|
||||
// for(Index var=0; var<varIndex.size(); ++var)
|
||||
// index_[var].insert(index_[var].begin(), varIndex[var].begin(), varIndex[var].end());
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Storage>
|
||||
template<class FactorGraph>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,104 @@
|
|||
/**
|
||||
* @file testEliminationTree.cpp
|
||||
* @brief
|
||||
* @author Richard Roberts
|
||||
* @created Oct 14, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
typedef EliminationTree<SymbolicFactorGraph> SymbolicEliminationTree;
|
||||
|
||||
struct EliminationTreeTester {
|
||||
// build hardcoded tree
|
||||
static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) {
|
||||
|
||||
SymbolicEliminationTree::shared_ptr leaf0(new SymbolicEliminationTree);
|
||||
leaf0->add(fg[0]);
|
||||
leaf0->add(fg[1]);
|
||||
|
||||
SymbolicEliminationTree::shared_ptr node1(new SymbolicEliminationTree(1));
|
||||
node1->add(fg[2]);
|
||||
node1->add(leaf0);
|
||||
|
||||
SymbolicEliminationTree::shared_ptr node2(new SymbolicEliminationTree(2));
|
||||
node2->add(fg[3]);
|
||||
node2->add(node1);
|
||||
|
||||
SymbolicEliminationTree::shared_ptr leaf3(new SymbolicEliminationTree(3));
|
||||
leaf3->add(fg[4]);
|
||||
|
||||
SymbolicEliminationTree::shared_ptr etree(new SymbolicEliminationTree(4));
|
||||
etree->add(leaf3);
|
||||
etree->add(node2);
|
||||
|
||||
return etree;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(EliminationTree, Create)
|
||||
{
|
||||
// create example factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg);
|
||||
|
||||
// Build from factor graph
|
||||
SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg);
|
||||
|
||||
CHECK(assert_equal(*expected,*actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test to drive elimination tree development
|
||||
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST(EliminationTree, eliminate )
|
||||
{
|
||||
// create expected Chordal bayes Net
|
||||
Conditional::shared_ptr c0(new Conditional(0, 1, 2));
|
||||
Conditional::shared_ptr c1(new Conditional(1, 2, 4));
|
||||
Conditional::shared_ptr c2(new Conditional(2, 4));
|
||||
Conditional::shared_ptr c3(new Conditional(3, 4));
|
||||
Conditional::shared_ptr c4(new Conditional(4));
|
||||
|
||||
SymbolicBayesNet expected;
|
||||
expected.push_back(c3);
|
||||
expected.push_back(c0);
|
||||
expected.push_back(c1);
|
||||
expected.push_back(c2);
|
||||
expected.push_back(c4);
|
||||
|
||||
// Create factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
// eliminate
|
||||
SymbolicBayesNet actual = *SymbolicEliminationTree::Create(fg)->eliminate();
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -102,297 +102,296 @@ TEST( SymbolicFactorGraph, push_back )
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
/**
|
||||
* An elimination tree is a tree of factors
|
||||
*/
|
||||
class ETree: public Testable<ETree> {
|
||||
///**
|
||||
// * An elimination tree is a tree of factors
|
||||
// */
|
||||
//class ETree: public Testable<ETree> {
|
||||
//
|
||||
//public:
|
||||
//
|
||||
// typedef boost::shared_ptr<Factor> sharedFactor;
|
||||
// typedef boost::shared_ptr<ETree> shared_ptr;
|
||||
//
|
||||
//private:
|
||||
//
|
||||
// Index key_; /** index associated with root */
|
||||
// list<sharedFactor> factors_; /** factors associated with root */
|
||||
// list<shared_ptr> subTrees_; /** sub-trees */
|
||||
//
|
||||
// typedef pair<SymbolicBayesNet, Factor> Result;
|
||||
//
|
||||
// /**
|
||||
// * Recursive routine that eliminates the factors arranged in an elimination tree
|
||||
// */
|
||||
// Result eliminate_() const {
|
||||
//
|
||||
// SymbolicBayesNet bn;
|
||||
//
|
||||
// set<Index> separator;
|
||||
//
|
||||
// // loop over all factors associated with root
|
||||
// // and set-union their keys to a separator
|
||||
// BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
// BOOST_FOREACH(Index key, *factor) {
|
||||
// if (key != key_) separator.insert(key); }
|
||||
//
|
||||
// // for all children, eliminate into Bayes net
|
||||
// BOOST_FOREACH(const shared_ptr& child, subTrees_) {
|
||||
// Result result = child->eliminate_();
|
||||
// bn.push_back(result.first);
|
||||
// BOOST_FOREACH(Index key, result.second)
|
||||
// if (key != key_) separator.insert(key);
|
||||
// }
|
||||
//
|
||||
// // Make the conditional from the key and separator, and insert it in Bayes net
|
||||
// vector<Index> parents;
|
||||
// std::copy(separator.begin(), separator.end(), back_inserter(parents));
|
||||
// Conditional::shared_ptr conditional(new Conditional(key_, parents));
|
||||
// bn.push_back(conditional);
|
||||
//
|
||||
// // now create the new factor from separator to return to caller
|
||||
// Factor newFactor(separator.begin(), separator.end());
|
||||
// return Result(bn, newFactor);
|
||||
// }
|
||||
//
|
||||
//public:
|
||||
//
|
||||
// /** default constructor */
|
||||
// ETree(Index key = 0) :
|
||||
// key_(key) {
|
||||
// }
|
||||
//
|
||||
// /** add a factor */
|
||||
// void add(const sharedFactor& factor) {
|
||||
// factors_.push_back(factor);
|
||||
// }
|
||||
//
|
||||
// /** add a subtree */
|
||||
// void add(const shared_ptr& child) {
|
||||
// subTrees_.push_back(child);
|
||||
// }
|
||||
//
|
||||
// void print(const std::string& name) const {
|
||||
// cout << name << " (" << key_ << ")" << endl;
|
||||
// BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
// factor->print(name + " ");
|
||||
// BOOST_FOREACH(const shared_ptr& child, subTrees_)
|
||||
// child->print(name + " ");
|
||||
// }
|
||||
//
|
||||
// bool equals(const ETree& expected, double tol) const {
|
||||
// // todo
|
||||
// return false;
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * Eliminate the factors to a Bayes Net
|
||||
// */
|
||||
// SymbolicBayesNet eliminate() const {
|
||||
//
|
||||
// // call recursive routine
|
||||
// Result result = eliminate_();
|
||||
// return result.first;
|
||||
// }
|
||||
//
|
||||
//};
|
||||
//
|
||||
//// build hardcoded tree
|
||||
//ETree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) {
|
||||
//
|
||||
// ETree::shared_ptr leaf0(new ETree);
|
||||
// leaf0->add(fg[0]);
|
||||
// leaf0->add(fg[1]);
|
||||
//
|
||||
// ETree::shared_ptr node1(new ETree(1));
|
||||
// node1->add(fg[2]);
|
||||
// node1->add(leaf0);
|
||||
//
|
||||
// ETree::shared_ptr node2(new ETree(2));
|
||||
// node2->add(fg[3]);
|
||||
// node2->add(node1);
|
||||
//
|
||||
// ETree::shared_ptr leaf3(new ETree(3));
|
||||
// leaf3->add(fg[4]);
|
||||
//
|
||||
// ETree::shared_ptr etree(new ETree(4));
|
||||
// etree->add(leaf3);
|
||||
// etree->add(node2);
|
||||
//
|
||||
// return etree;
|
||||
//}
|
||||
//
|
||||
//typedef size_t RowIndex;
|
||||
//typedef vector<list<RowIndex> > StructA;
|
||||
//typedef vector<boost::optional<Index> > optionalIndices;
|
||||
//
|
||||
///**
|
||||
// * Gilbert01bit algorithm in Figure 2.2
|
||||
// */
|
||||
//optionalIndices buildETree(const StructA& structA) {
|
||||
//
|
||||
// // todo: get n
|
||||
// size_t n = 5;
|
||||
// optionalIndices parent(n);
|
||||
//
|
||||
// // todo: get m
|
||||
// size_t m = 5;
|
||||
// optionalIndices prev_col(m);
|
||||
//
|
||||
// // for column j \in 1 to n do
|
||||
// for (Index j = 0; j < n; j++) {
|
||||
// // for row i \in Struct[A*j] do
|
||||
// BOOST_FOREACH(RowIndex i, structA[j]) {
|
||||
// if (prev_col[i]) {
|
||||
// Index k = *(prev_col[i]);
|
||||
// // find root r of the current tree that contains k
|
||||
// Index r = k;
|
||||
// while (parent[r])
|
||||
// r = *parent[r];
|
||||
// if (r != j) parent[r].reset(j);
|
||||
// }
|
||||
// prev_col[i].reset(j);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// return parent;
|
||||
//}
|
||||
//
|
||||
///**
|
||||
// * TODO: Build StructA from factor graph
|
||||
// */
|
||||
//StructA createStructA(const SymbolicFactorGraph& fg) {
|
||||
//
|
||||
// StructA structA;
|
||||
//
|
||||
// // hardcode for now
|
||||
// list<RowIndex> list0;
|
||||
// list0 += 0, 1;
|
||||
// structA.push_back(list0);
|
||||
// list<RowIndex> list1;
|
||||
// list1 += 0, 2;
|
||||
// structA.push_back(list1);
|
||||
// list<RowIndex> list2;
|
||||
// list2 += 1, 3;
|
||||
// structA.push_back(list2);
|
||||
// list<RowIndex> list3;
|
||||
// list3 += 4;
|
||||
// structA.push_back(list3);
|
||||
// list<RowIndex> list4;
|
||||
// list4 += 2, 3, 4;
|
||||
// structA.push_back(list4);
|
||||
//
|
||||
// return structA;
|
||||
//}
|
||||
//
|
||||
///**
|
||||
// * Build ETree from factor graph and parent indices
|
||||
// */
|
||||
//ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg, const optionalIndices& parent) {
|
||||
//
|
||||
// // todo: get n
|
||||
// size_t n = 5;
|
||||
//
|
||||
// // Create tree structure
|
||||
// vector<ETree::shared_ptr> trees(n);
|
||||
// for (Index k = 1; k <= n; k++) {
|
||||
// Index j = n - k;
|
||||
// trees[j].reset(new ETree(j));
|
||||
// if (parent[j]) trees[*parent[j]]->add(trees[j]);
|
||||
// }
|
||||
//
|
||||
// // Hang factors in right places
|
||||
// BOOST_FOREACH(const ETree::sharedFactor& factor, fg)
|
||||
// {
|
||||
// Index j = factor->front();
|
||||
// trees[j]->add(factor);
|
||||
// }
|
||||
//
|
||||
// return trees[n - 1];
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// Test of elimination tree creation
|
||||
//// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
|
||||
///* ************************************************************************* */
|
||||
///**
|
||||
// * Build ETree from factor graph
|
||||
// */
|
||||
//ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg) {
|
||||
//
|
||||
// // create vector of factor indices
|
||||
// StructA structA = createStructA(fg);
|
||||
//
|
||||
// // call Gilbert01bit algorithm
|
||||
// optionalIndices parent = buildETree(structA);
|
||||
//
|
||||
// // Build ETree from factor graph and parent indices
|
||||
// return buildETree(fg, parent);
|
||||
//}
|
||||
//
|
||||
//TEST( ETree, buildETree )
|
||||
//{
|
||||
// // create example factor graph
|
||||
// SymbolicFactorGraph fg;
|
||||
// fg.push_factor(0, 1);
|
||||
// fg.push_factor(0, 2);
|
||||
// fg.push_factor(1, 4);
|
||||
// fg.push_factor(2, 4);
|
||||
// fg.push_factor(3, 4);
|
||||
//
|
||||
// ETree::shared_ptr expected = buildHardcodedTree(fg);
|
||||
//
|
||||
// // Build from factor graph
|
||||
// ETree::shared_ptr actual = buildETree(fg);
|
||||
//
|
||||
// // todo: CHECK(assert_equal(*expected,*actual));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// Test to drive elimination tree development
|
||||
//// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
|
||||
///* ************************************************************************* */
|
||||
//
|
||||
///**
|
||||
// * Eliminate factor graph
|
||||
// */
|
||||
//SymbolicBayesNet eliminate(const SymbolicFactorGraph& fg) {
|
||||
//
|
||||
// // build etree
|
||||
// ETree::shared_ptr etree = buildETree(fg);
|
||||
//
|
||||
// return etree->eliminate();
|
||||
//}
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Factor> sharedFactor;
|
||||
typedef boost::shared_ptr<ETree> shared_ptr;
|
||||
|
||||
private:
|
||||
|
||||
Index key_; /** index associated with root */
|
||||
list<sharedFactor> factors_; /** factors associated with root */
|
||||
list<shared_ptr> subTrees_; /** sub-trees */
|
||||
|
||||
typedef pair<SymbolicBayesNet, Factor> Result;
|
||||
|
||||
/**
|
||||
* Recursive routine that eliminates the factors arranged in an elimination tree
|
||||
*/
|
||||
Result eliminate_() const {
|
||||
|
||||
SymbolicBayesNet bn;
|
||||
|
||||
set<Index> separator;
|
||||
|
||||
// loop over all factors associated with root
|
||||
// and set-union their keys to a separator
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
BOOST_FOREACH(Index key, *factor)
|
||||
if (key != key_) separator.insert(key);
|
||||
|
||||
// for all children, eliminate into Bayes net
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_)
|
||||
{
|
||||
Result result = child->eliminate_();
|
||||
bn.push_back(result.first);
|
||||
BOOST_FOREACH(Index key, result.second)
|
||||
if (key != key_) separator.insert(key);
|
||||
}
|
||||
|
||||
// Make the conditional from the key and separator, and insert it in Bayes net
|
||||
vector<Index> parents;
|
||||
std::copy(separator.begin(), separator.end(), back_inserter(parents));
|
||||
Conditional::shared_ptr conditional(new Conditional(key_, parents));
|
||||
bn.push_back(conditional);
|
||||
|
||||
// now create the new factor from separator to return to caller
|
||||
Factor newFactor(separator.begin(), separator.end());
|
||||
return Result(bn, newFactor);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor */
|
||||
ETree(Index key = 0) :
|
||||
key_(key) {
|
||||
}
|
||||
|
||||
/** add a factor */
|
||||
void add(const sharedFactor& factor) {
|
||||
factors_.push_back(factor);
|
||||
}
|
||||
|
||||
/** add a subtree */
|
||||
void add(const shared_ptr& child) {
|
||||
subTrees_.push_back(child);
|
||||
}
|
||||
|
||||
void print(const std::string& name) const {
|
||||
cout << name << " (" << key_ << ")" << endl;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
factor->print(name + " ");
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_)
|
||||
child->print(name + " ");
|
||||
}
|
||||
|
||||
bool equals(const ETree& expected, double tol) const {
|
||||
// todo
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Eliminate the factors to a Bayes Net
|
||||
*/
|
||||
SymbolicBayesNet eliminate() const {
|
||||
|
||||
// call recursive routine
|
||||
Result result = eliminate_();
|
||||
return result.first;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
// build hardcoded tree
|
||||
ETree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) {
|
||||
|
||||
ETree::shared_ptr leaf0(new ETree);
|
||||
leaf0->add(fg[0]);
|
||||
leaf0->add(fg[1]);
|
||||
|
||||
ETree::shared_ptr node1(new ETree(1));
|
||||
node1->add(fg[2]);
|
||||
node1->add(leaf0);
|
||||
|
||||
ETree::shared_ptr node2(new ETree(2));
|
||||
node2->add(fg[3]);
|
||||
node2->add(node1);
|
||||
|
||||
ETree::shared_ptr leaf3(new ETree(3));
|
||||
leaf3->add(fg[4]);
|
||||
|
||||
ETree::shared_ptr etree(new ETree(4));
|
||||
etree->add(leaf3);
|
||||
etree->add(node2);
|
||||
|
||||
return etree;
|
||||
}
|
||||
|
||||
typedef size_t RowIndex;
|
||||
typedef vector<list<RowIndex> > StructA;
|
||||
typedef vector<boost::optional<Index> > optionalIndices;
|
||||
|
||||
/**
|
||||
* Gilbert01bit algorithm in Figure 2.2
|
||||
*/
|
||||
optionalIndices buildETree(const StructA& structA) {
|
||||
|
||||
// todo: get n
|
||||
size_t n = 5;
|
||||
optionalIndices parent(n);
|
||||
|
||||
// todo: get m
|
||||
size_t m = 5;
|
||||
optionalIndices prev_col(m);
|
||||
|
||||
// for column j \in 1 to n do
|
||||
for (Index j = 0; j < n; j++) {
|
||||
// for row i \in Struct[A*j] do
|
||||
BOOST_FOREACH(RowIndex i, structA[j]) {
|
||||
if (prev_col[i]) {
|
||||
Index k = *(prev_col[i]);
|
||||
// find root r of the current tree that contains k
|
||||
Index r = k;
|
||||
while (parent[r])
|
||||
r = *parent[r];
|
||||
if (r != j) parent[r].reset(j);
|
||||
}
|
||||
prev_col[i].reset(j);
|
||||
}
|
||||
}
|
||||
|
||||
return parent;
|
||||
}
|
||||
|
||||
/**
|
||||
* TODO: Build StructA from factor graph
|
||||
*/
|
||||
StructA createStructA(const SymbolicFactorGraph& fg) {
|
||||
|
||||
StructA structA;
|
||||
|
||||
// hardcode for now
|
||||
list<RowIndex> list0;
|
||||
list0 += 0, 1;
|
||||
structA.push_back(list0);
|
||||
list<RowIndex> list1;
|
||||
list1 += 0, 2;
|
||||
structA.push_back(list1);
|
||||
list<RowIndex> list2;
|
||||
list2 += 1, 3;
|
||||
structA.push_back(list2);
|
||||
list<RowIndex> list3;
|
||||
list3 += 4;
|
||||
structA.push_back(list3);
|
||||
list<RowIndex> list4;
|
||||
list4 += 2, 3, 4;
|
||||
structA.push_back(list4);
|
||||
|
||||
return structA;
|
||||
}
|
||||
|
||||
/**
|
||||
* Build ETree from factor graph and parent indices
|
||||
*/
|
||||
ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg, const optionalIndices& parent) {
|
||||
|
||||
// todo: get n
|
||||
size_t n = 5;
|
||||
|
||||
// Create tree structure
|
||||
vector<ETree::shared_ptr> trees(n);
|
||||
for (Index k = 1; k <= n; k++) {
|
||||
Index j = n - k;
|
||||
trees[j].reset(new ETree(j));
|
||||
if (parent[j]) trees[*parent[j]]->add(trees[j]);
|
||||
}
|
||||
|
||||
// Hang factors in right places
|
||||
BOOST_FOREACH(const ETree::sharedFactor& factor, fg)
|
||||
{
|
||||
Index j = factor->front();
|
||||
trees[j]->add(factor);
|
||||
}
|
||||
|
||||
return trees[n - 1];
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test of elimination tree creation
|
||||
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Build ETree from factor graph
|
||||
*/
|
||||
ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg) {
|
||||
|
||||
// create vector of factor indices
|
||||
StructA structA = createStructA(fg);
|
||||
|
||||
// call Gilbert01bit algorithm
|
||||
optionalIndices parent = buildETree(structA);
|
||||
|
||||
// Build ETree from factor graph and parent indices
|
||||
return buildETree(fg, parent);
|
||||
}
|
||||
|
||||
TEST( ETree, buildETree )
|
||||
{
|
||||
// create example factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
ETree::shared_ptr expected = buildHardcodedTree(fg);
|
||||
|
||||
// Build from factor graph
|
||||
ETree::shared_ptr actual = buildETree(fg);
|
||||
|
||||
// todo: CHECK(assert_equal(*expected,*actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test to drive elimination tree development
|
||||
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
|
||||
/* ************************************************************************* */
|
||||
|
||||
/**
|
||||
* Eliminate factor graph
|
||||
*/
|
||||
SymbolicBayesNet eliminate(const SymbolicFactorGraph& fg) {
|
||||
|
||||
// build etree
|
||||
ETree::shared_ptr etree = buildETree(fg);
|
||||
|
||||
return etree->eliminate();
|
||||
}
|
||||
|
||||
TEST( SymbolicFactorGraph, eliminate )
|
||||
{
|
||||
// create expected Chordal bayes Net
|
||||
Conditional::shared_ptr c0(new Conditional(0, 1, 2));
|
||||
Conditional::shared_ptr c1(new Conditional(1, 2, 4));
|
||||
Conditional::shared_ptr c2(new Conditional(2, 4));
|
||||
Conditional::shared_ptr c3(new Conditional(3, 4));
|
||||
Conditional::shared_ptr c4(new Conditional(4));
|
||||
|
||||
SymbolicBayesNet expected;
|
||||
expected.push_back(c3);
|
||||
expected.push_back(c0);
|
||||
expected.push_back(c1);
|
||||
expected.push_back(c2);
|
||||
expected.push_back(c4);
|
||||
|
||||
// Create factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
// eliminate
|
||||
SymbolicBayesNet actual = eliminate(fg);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
//TEST( SymbolicFactorGraph, eliminate )
|
||||
//{
|
||||
// // create expected Chordal bayes Net
|
||||
// Conditional::shared_ptr c0(new Conditional(0, 1, 2));
|
||||
// Conditional::shared_ptr c1(new Conditional(1, 2, 4));
|
||||
// Conditional::shared_ptr c2(new Conditional(2, 4));
|
||||
// Conditional::shared_ptr c3(new Conditional(3, 4));
|
||||
// Conditional::shared_ptr c4(new Conditional(4));
|
||||
//
|
||||
// SymbolicBayesNet expected;
|
||||
// expected.push_back(c3);
|
||||
// expected.push_back(c0);
|
||||
// expected.push_back(c1);
|
||||
// expected.push_back(c2);
|
||||
// expected.push_back(c4);
|
||||
//
|
||||
// // Create factor graph
|
||||
// SymbolicFactorGraph fg;
|
||||
// fg.push_factor(0, 1);
|
||||
// fg.push_factor(0, 2);
|
||||
// fg.push_factor(1, 4);
|
||||
// fg.push_factor(2, 4);
|
||||
// fg.push_factor(3, 4);
|
||||
//
|
||||
// // eliminate
|
||||
// SymbolicBayesNet actual = eliminate(fg);
|
||||
//
|
||||
// CHECK(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
|
|
@ -715,24 +715,24 @@ TEST(GaussianFactor, permuteWithInverse)
|
|||
GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual)));
|
||||
GaussianVariableIndex<> actualIndex(actualFG);
|
||||
actual.permuteWithInverse(inversePermutation);
|
||||
actualIndex.permute(*inversePermutation.inverse());
|
||||
// actualIndex.permute(*inversePermutation.inverse());
|
||||
|
||||
GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
|
||||
GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected)));
|
||||
GaussianVariableIndex<> expectedIndex(expectedFG);
|
||||
// GaussianVariableIndex<> expectedIndex(expectedFG);
|
||||
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
// todo: fix this!!! VariableIndex should not hold slots
|
||||
for(Index j=0; j<actualIndex.size(); ++j) {
|
||||
BOOST_FOREACH( GaussianVariableIndex<>::mapped_factor_type& factor_pos, actualIndex[j]) {
|
||||
factor_pos.variablePosition = numeric_limits<Index>::max(); }
|
||||
}
|
||||
for(Index j=0; j<expectedIndex.size(); ++j) {
|
||||
BOOST_FOREACH( GaussianVariableIndex<>::mapped_factor_type& factor_pos, expectedIndex[j]) {
|
||||
factor_pos.variablePosition = numeric_limits<Index>::max(); }
|
||||
}
|
||||
CHECK(assert_equal(expectedIndex, actualIndex));
|
||||
// // todo: fix this!!! VariableIndex should not hold slots
|
||||
// for(Index j=0; j<actualIndex.size(); ++j) {
|
||||
// BOOST_FOREACH( GaussianVariableIndex<>::mapped_factor_type& factor_pos, actualIndex[j]) {
|
||||
// factor_pos.variablePosition = numeric_limits<Index>::max(); }
|
||||
// }
|
||||
// for(Index j=0; j<expectedIndex.size(); ++j) {
|
||||
// BOOST_FOREACH( GaussianVariableIndex<>::mapped_factor_type& factor_pos, expectedIndex[j]) {
|
||||
// factor_pos.variablePosition = numeric_limits<Index>::max(); }
|
||||
// }
|
||||
// CHECK(assert_equal(expectedIndex, actualIndex));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -60,8 +60,7 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Values>
|
||||
pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr>
|
||||
NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const {
|
||||
Ordering::shared_ptr NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const {
|
||||
|
||||
// Create symbolic graph and initial (iterator) ordering
|
||||
FactorGraph<Factor>::shared_ptr symbolic;
|
||||
|
|
@ -76,14 +75,12 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
|
|||
|
||||
// Permute the Ordering and VariableIndex with the COLAMD ordering
|
||||
ordering->permuteWithInverse(*colamdPerm->inverse());
|
||||
variableIndex.permute(*colamdPerm);
|
||||
|
||||
// Build a variable dimensions array to upgrade to a GaussianVariableIndex
|
||||
GaussianVariableIndex<>::shared_ptr gaussianVarIndex(new GaussianVariableIndex<>(variableIndex, config.dims(*ordering)));
|
||||
// variableIndex.permute(*colamdPerm);
|
||||
// SL-FIX: fix permutation
|
||||
|
||||
// Return the Ordering and VariableIndex to be re-used during linearization
|
||||
// and elimination
|
||||
return make_pair(ordering, gaussianVarIndex);
|
||||
return ordering;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -84,8 +84,7 @@ namespace gtsam {
|
|||
* ordering and a VariableIndex, which can later be re-used to save
|
||||
* computation.
|
||||
*/
|
||||
std::pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr>
|
||||
orderingCOLAMD(const Values& config) const;
|
||||
Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
|
||||
|
||||
/**
|
||||
* linearize a nonlinear factor graph
|
||||
|
|
|
|||
|
|
@ -230,9 +230,7 @@ namespace gtsam {
|
|||
verbosityLevel verbosity = SILENT) {
|
||||
|
||||
// Use a variable ordering from COLAMD
|
||||
Ordering::shared_ptr ordering;
|
||||
GaussianVariableIndex<>::shared_ptr variableIndex;
|
||||
boost::tie(ordering, variableIndex) = graph->orderingCOLAMD(*config);
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*config);
|
||||
|
||||
double relativeThreshold = 1e-5, absoluteThreshold = 1e-5;
|
||||
|
||||
|
|
@ -264,9 +262,7 @@ namespace gtsam {
|
|||
*/
|
||||
static shared_values optimizeGN(shared_graph graph, shared_values config,
|
||||
verbosityLevel verbosity = SILENT) {
|
||||
Ordering::shared_ptr ordering;
|
||||
GaussianVariableIndex<>::shared_ptr variableIndex;
|
||||
boost::tie(ordering, variableIndex) = graph->orderingCOLAMD(*config);
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*config);
|
||||
double relativeThreshold = 1e-5, absoluteThreshold = 1e-5;
|
||||
|
||||
// initial optimization state is the same in both cases tested
|
||||
|
|
|
|||
|
|
@ -175,9 +175,7 @@ TEST( Graph, CHECK_ORDERING)
|
|||
initialEstimate->insert(3, landmark3);
|
||||
initialEstimate->insert(4, landmark4);
|
||||
|
||||
Ordering::shared_ptr ordering;
|
||||
GaussianVariableIndex<>::shared_ptr varindex;
|
||||
boost::tie(ordering,varindex) = graph->orderingCOLAMD(*initialEstimate);
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ using namespace boost::assign;
|
|||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
//#include <gtsam/inference/BayesTree-inl.h>
|
||||
#include <gtsam/inference/inference-inl.h>
|
||||
|
||||
#include <gtsam/inference/EliminationTree-inl.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace example;
|
||||
|
|
@ -313,6 +313,9 @@ TEST( GaussianFactorGraph, eliminateAll )
|
|||
GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
|
||||
GaussianBayesNet actual = *Inference::Eliminate(fg1);
|
||||
CHECK(assert_equal(expected,actual,tol));
|
||||
|
||||
GaussianBayesNet actualET = *EliminationTree<GaussianFactorGraph>::Create(fg1)->eliminate();
|
||||
CHECK(assert_equal(expected,actualET,tol));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
|
|
@ -488,11 +491,13 @@ TEST( GaussianFactorGraph, optimize )
|
|||
|
||||
// optimize the graph
|
||||
VectorValues actual = optimize(*Inference::Eliminate(fg));
|
||||
VectorValues actualET = optimize(*EliminationTree<GaussianFactorGraph>::Create(fg)->eliminate());
|
||||
|
||||
// verify
|
||||
VectorValues expected = createCorrectDelta(ord);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expected,actualET));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
|
|
@ -781,10 +786,12 @@ TEST( GaussianFactorGraph, constrained_simple )
|
|||
|
||||
// eliminate and solve
|
||||
VectorValues actual = optimize(*Inference::Eliminate(fg));
|
||||
VectorValues actualET = optimize(*EliminationTree<GaussianFactorGraph>::Create(fg)->eliminate());
|
||||
|
||||
// verify
|
||||
VectorValues expected = createSimpleConstraintValues();
|
||||
CHECK(assert_equal(expected, actual));
|
||||
CHECK(assert_equal(expected, actualET));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -169,7 +169,7 @@ TEST(GaussianJunctionTree, slamlike) {
|
|||
++ i;
|
||||
|
||||
// Compare solutions
|
||||
Ordering ordering = *fullgraph.orderingCOLAMD(init).first;
|
||||
Ordering ordering = *fullgraph.orderingCOLAMD(init);
|
||||
GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering);
|
||||
|
||||
GaussianJunctionTree gjt(linearized);
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ TEST( Graph, GET_ORDERING)
|
|||
{
|
||||
Ordering expected; expected += "x1","l1","x2";
|
||||
Graph nlfg = createNonlinearFactorGraph();
|
||||
Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues()).first;
|
||||
Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues());
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ int main(int argc, char *argv[]) {
|
|||
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
|
||||
|
||||
tic("Z 1 order");
|
||||
Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second).first);
|
||||
Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second));
|
||||
toc("Z 1 order");
|
||||
|
||||
tic("Z 2 linearize");
|
||||
|
|
|
|||
Loading…
Reference in New Issue