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

New linear solver interface, global inference functions removed.

release/4.3a0
Richard Roberts 2010-10-21 22:59:54 +00:00
parent b9ec67cc3f
commit 812e3277ee
39 changed files with 442 additions and 936 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="false" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">

2
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value></value>
<value>-j2</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>

2
base/Makefile.am
View File

@ -67,7 +67,7 @@ endif
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_DEFAULT_SOURCE_EXT = .cpp
AM_LDFLAGS = $(boost_serialization) -L$(CCOLAMDLib) -lccolamd
AM_LDFLAGS += $(boost_serialization) -L$(CCOLAMDLib) -lccolamd
LDADD = libbase.la ../CppUnitLite/libCppUnitLite.a
if USE_BLAS_LINUX

3
inference/BayesNet.h
View File

@ -94,7 +94,8 @@ namespace gtsam {
/** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation);
/** Permute the variables when only separator variables need to be permuted.
/**
* Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/

144
inference/BayesTree-inl.h
View File

@ -32,8 +32,10 @@
using namespace boost::assign;
namespace lam = boost::lambda;
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/GenericSequentialSolver.h>
namespace gtsam {
@ -266,9 +268,11 @@ namespace gtsam {
// TODO, why do we actually return a shared pointer, why does eliminate?
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
BayesNet<CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R) {
static const bool debug = false;
// A first base case is when this clique or its parent is the root,
// in which case we return an empty Bayes net.
@ -279,15 +283,21 @@ namespace gtsam {
return empty;
}
// The root conditional
FactorGraph<typename CONDITIONAL::Factor> p_R(*R);
// The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form
FACTORGRAPH p_Fp_Sp(*parent);
FactorGraph<typename CONDITIONAL::Factor> p_Fp_Sp(*parent);
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
FACTORGRAPH p_Sp_R(parent->shortcut<FACTORGRAPH>(R));
FactorGraph<typename CONDITIONAL::Factor> p_Sp_R(parent->shortcut(R));
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
FACTORGRAPH p_Cp_R = combine(p_Fp_Sp, p_Sp_R);
FactorGraph<typename CONDITIONAL::Factor> p_Cp_R;
p_Cp_R.push_back(p_R);
p_Cp_R.push_back(p_Fp_Sp);
p_Cp_R.push_back(p_Sp_R);
// Eliminate into a Bayes net with ordering designed to integrate out
// any variables not in *our* separator. Variables to integrate out must be
@ -295,45 +305,48 @@ namespace gtsam {
// However, an added wrinkle is that Cp might overlap with the root.
// Keys corresponding to the root should not be added to the ordering at all.
typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > FastJSet;
// Get the key list Cp=Fp+Sp, which will form the basis for the integrands
vector<Index> parentKeys(parent->keys());
FastJSet integrands(parentKeys.begin(), parentKeys.end());
// Start ordering with the separator
FastJSet separator(separator_.begin(), separator_.end());
// remove any variables in the root, after this integrands = Cp\R, ordering = S\R
BOOST_FOREACH(Index key, R->ordering()) {
integrands.erase(key);
separator.erase(key);
if(debug) {
p_R.print("p_R: ");
p_Fp_Sp.print("p_Fp_Sp: ");
p_Sp_R.print("p_Sp_R: ");
}
// remove any variables in the separator, after this integrands = Cp\R\S
BOOST_FOREACH(Index key, separator_) integrands.erase(key);
// form the ordering as [Cp\R\S S\R]
vector<Index> ordering; ordering.reserve(integrands.size() + separator.size());
BOOST_FOREACH(Index key, integrands) ordering.push_back(key);
BOOST_FOREACH(Index key, separator) ordering.push_back(key);
// eliminate to get marginal
typename FACTORGRAPH::variableindex_type varIndex(p_Cp_R);
Permutation toFront = Permutation::PullToFront(ordering, varIndex.size());
Permutation::shared_ptr toFrontInverse(toFront.inverse());
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, p_Cp_R) {
factor->permuteWithInverse(*toFrontInverse);
// We want to factor into a conditional of the clique variables given the
// root and the marginal on the root, integrating out all other variables.
// The integrands include any parents of this clique and the variables of
// the parent clique.
vector<Index> variablesAtBack;
variablesAtBack.reserve(this->size() + R->size());
BOOST_FOREACH(const Index separatorIndex, this->separator_) {
variablesAtBack.push_back(separatorIndex);
if(debug) cout << "At back (this): " << separatorIndex << endl;
}
BOOST_FOREACH(const sharedConditional& conditional, *R) {
variablesAtBack.push_back(conditional->key());
if(debug) cout << "At back (root): " << conditional->key() << endl;
}
varIndex.permute(toFront);
BayesNet<CONDITIONAL> p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex);
// remove all integrands
for(Index j=0; j<integrands.size(); ++j)
p_S_R.pop_front();
Permutation toBack = Permutation::PushToBack(variablesAtBack, R->back()->key() + 1);
Permutation::shared_ptr toBackInverse(toBack.inverse());
BOOST_FOREACH(const typename CONDITIONAL::Factor::shared_ptr& factor, p_Cp_R) {
factor->permuteWithInverse(*toBackInverse); }
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<typename CONDITIONAL::Factor>::Create(p_Cp_R)->eliminate());
// take only the conditionals for p(S|R)
BayesNet<CONDITIONAL> p_S_R;
typename BayesNet<CONDITIONAL>::const_reverse_iterator conditional = eliminated->rbegin();
BOOST_FOREACH(const sharedConditional& c, *R) {
(void)c; ++conditional; }
BOOST_FOREACH(const Index c, this->separator_) {
if(debug)
(*conditional)->print("Taking C|R conditional: ");
(void)c; p_S_R.push_front(*(conditional++)); }
// for(Index j=0; j<integrands.size(); ++j)
// p_S_R.pop_front();
// Undo the permutation on the shortcut
p_S_R.permuteWithInverse(toFront);
p_S_R.permuteWithInverse(toBack);
// return the parent shortcut P(Sp|R)
return p_S_R;
@ -346,18 +359,19 @@ namespace gtsam {
// Because the root clique could be very big.
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
FACTORGRAPH BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
FactorGraph<typename CONDITIONAL::Factor> BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
// If we are the root, just return this root
if (R.get()==this) return *R;
// Combine P(F|S), P(S|R), and P(R)
BayesNet<CONDITIONAL> p_FSR = this->shortcut<FACTORGRAPH>(R);
BayesNet<CONDITIONAL> p_FSR = this->shortcut(R);
p_FSR.push_front(*this);
p_FSR.push_back(*R);
// Find marginal on the keys we are interested in
return Inference::Marginal(FACTORGRAPH(p_FSR), keys());
FactorGraph<typename CONDITIONAL::Factor> p_FSRfg(p_FSR);
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).joint(keys());
}
// /* ************************************************************************* */
@ -689,44 +703,46 @@ namespace gtsam {
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
FACTORGRAPH BayesTree<CONDITIONAL>::marginal(Index key) const {
typename CONDITIONAL::Factor::shared_ptr BayesTree<CONDITIONAL>::marginal(Index key) const {
// get clique containing key
sharedClique clique = (*this)[key];
// calculate or retrieve its marginal
FACTORGRAPH cliqueMarginal = clique->marginal<FACTORGRAPH>(root_);
FactorGraph<typename CONDITIONAL::Factor> cliqueMarginal = clique->marginal(root_);
// Reorder so that only the requested key is not eliminated
typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal);
vector<Index> keyAsVector(1); keyAsVector[0] = key;
Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size()));
Permutation::shared_ptr toBackInverse(toBack.inverse());
varIndex.permute(toBack);
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
factor->permuteWithInverse(*toBackInverse);
}
return GenericSequentialSolver<typename CONDITIONAL::Factor>(cliqueMarginal).marginal(key);
// partially eliminate, remaining factor graph is requested marginal
Inference::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex);
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
if(factor)
factor->permuteWithInverse(toBack);
}
return cliqueMarginal;
// // Reorder so that only the requested key is not eliminated
// typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal);
// vector<Index> keyAsVector(1); keyAsVector[0] = key;
// Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size()));
// Permutation::shared_ptr toBackInverse(toBack.inverse());
// varIndex.permute(toBack);
// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
// factor->permuteWithInverse(*toBackInverse);
// }
//
// // partially eliminate, remaining factor graph is requested marginal
// SymbolicSequentialSolver::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex);
// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
// if(factor)
// factor->permuteWithInverse(toBack);
// }
// return cliqueMarginal;
}
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
// calculate marginal as a factor graph
FACTORGRAPH fg = this->marginal<FACTORGRAPH>(key);
typename FactorGraph<typename CONDITIONAL::Factor>::shared_ptr fg(
new FactorGraph<typename CONDITIONAL::Factor>());
fg->push_back(this->marginal(key));
// eliminate further to Bayes net
return *Inference::Eliminate(fg);
return GenericSequentialSolver<typename CONDITIONAL::Factor>(*fg).eliminate();
}
// /* ************************************************************************* */

10
inference/BayesTree.h
View File

@ -107,12 +107,10 @@ namespace gtsam {
/** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version
template<class FACTORGRAPH>
BayesNet<CONDITIONAL> shortcut(shared_ptr root);
/** return the marginal P(C) of the clique */
template<class FACTORGRAPH>
FACTORGRAPH marginal(shared_ptr root);
FactorGraph<typename CONDITIONAL::Factor> marginal(shared_ptr root);
// /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
// template<class Factor>
@ -257,12 +255,10 @@ namespace gtsam {
CliqueData getCliqueData() const;
/** return marginal on any variable */
template<class FACTORGRAPH>
FACTORGRAPH marginal(Index key) const;
typename CONDITIONAL::Factor::shared_ptr marginal(Index key) const;
/** return marginal on any variable, as a Bayes Net */
template<class FACTORGRAPH>
BayesNet<CONDITIONAL> marginalBayesNet(Index key) const;
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key) const;
// /** return joint on two variables */
// template<class Factor>

53
inference/EliminationTree-inl.h
View File

@ -21,11 +21,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTree<FACTOR>::EliminationResult
EliminationTree<FACTOR>::eliminate_() const {
typename EliminationTree<FACTOR>::sharedFactor
EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
// Create the Bayes net, which will be returned to the parent. Initially empty...
BayesNet bayesNet;
set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > separator;
// Create the list of factors to be eliminated, initially empty, and reserve space
FactorGraph<FACTOR> factors;
@ -36,19 +35,14 @@ EliminationTree<FACTOR>::eliminate_() const {
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
EliminationResult result = child->eliminate_();
bayesNet.push_back(result.first); // Bayes net fragment added to Bayes net
factors.push_back(result.second); // Separator factor added to [factors]
}
factors.push_back(child->eliminate_(conditionals)); }
// Combine all factors (from this node and from subtrees) into a joint factor
sharedFactor jointFactor(FACTOR::Combine(factors, VariableSlots(factors)));
assert(jointFactor->front() == this->key_);
conditionals[this->key_] = jointFactor->eliminateFirst();
// Eliminate the resulting joint factor and add the conditional to the bayes net
// What remains in the jointFactor will be passed to our parent node
bayesNet.push_back(jointFactor->eliminateFirst());
return EliminationResult(bayesNet, jointFactor);
return jointFactor;
}
/* ************************************************************************* */
@ -87,18 +81,15 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& str
/* ************************************************************************* */
template<class FACTOR>
template<class FACTORGRAPH>
template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FACTORGRAPH& factorGraph) {
// Create column structure
VariableIndex<> varIndex(factorGraph);
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex<>& structure) {
// Compute the tree structure
vector<Index> parents(ComputeParents(varIndex));
vector<Index> parents(ComputeParents(structure));
// Number of variables
const size_t n = varIndex.size();
const size_t n = structure.size();
static const Index none = numeric_limits<Index>::max();
@ -112,7 +103,7 @@ EliminationTree<FACTOR>::Create(const FACTORGRAPH& factorGraph) {
}
// Hang factors in right places
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& derivedFactor, factorGraph) {
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) {
// Here we static_cast to the factor type of this EliminationTree. This
// allows performing symbolic elimination on, for example, GaussianFactors.
sharedFactor factor(boost::shared_static_cast<FACTOR>(derivedFactor));
@ -129,6 +120,14 @@ EliminationTree<FACTOR>::Create(const FACTORGRAPH& factorGraph) {
return trees.back();
}
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
return Create(factorGraph, VariableIndex<>(factorGraph));
}
/* ************************************************************************* */
template<class FACTORGRAPH>
void EliminationTree<FACTORGRAPH>::print(const std::string& name) const {
@ -160,8 +159,16 @@ typename EliminationTree<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminate() const {
// call recursive routine
EliminationResult result = eliminate_();
return typename BayesNet::shared_ptr(new BayesNet(result.first));
Conditionals conditionals(this->key_ + 1);
(void)eliminate_(conditionals);
// Add conditionals to BayesNet
typename BayesNet::shared_ptr bayesNet(new BayesNet);
BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) {
if(conditional)
bayesNet->push_back(conditional);
}
return bayesNet;
}
}

17
inference/EliminationTree.h
View File

@ -13,6 +13,7 @@
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree
@ -35,13 +36,12 @@ private:
typedef std::list<sharedFactor, boost::fast_pool_allocator<sharedFactor> > Factors;
typedef std::list<shared_ptr, boost::fast_pool_allocator<shared_ptr> > SubTrees;
typedef std::vector<typename FACTOR::Conditional::shared_ptr> Conditionals;
Index key_; /** index associated with root */
Factors factors_; /** factors associated with root */
SubTrees subTrees_; /** sub-trees */
typedef std::pair<BayesNet, sharedFactor> EliminationResult;
/** default constructor, private, as you should use Create below */
EliminationTree(Index key = 0) : key_(key) {}
@ -60,16 +60,23 @@ private:
/**
* Recursive routine that eliminates the factors arranged in an elimination tree
*/
EliminationResult eliminate_() const;
sharedFactor eliminate_(Conditionals& conditionals) 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 using
* pre-computed column structure.
*/
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex<>& structure);
/** Named constructor to build the elimination tree of a factor graph */
template<class FACTORGRAPH>
static shared_ptr Create(const FACTORGRAPH& factorGraph);
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph);
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ") const;

2
inference/FactorGraph.h
View File

@ -160,7 +160,7 @@ namespace gtsam {
factors_.reserve(factorGraph.size());
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) {
if(factor)
this->push_back(sharedFactor(new FACTOR(*factor)));
this->push_back(factor);
else
this->push_back(sharedFactor());
}

3
inference/ISAM-inl.h
View File

@ -24,6 +24,7 @@ using namespace boost::assign;
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam {
@ -52,7 +53,7 @@ namespace gtsam {
factors.push_back(newFactors);
// eliminate into a Bayes net
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = Inference::Eliminate(factors);
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::Factor>(factors).eliminate();
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;

4
inference/JunctionTree-inl.h
View File

@ -24,7 +24,7 @@
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/VariableSlots-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <boost/foreach.hpp>
#include <boost/pool/pool_alloc.hpp>
@ -42,7 +42,7 @@ namespace gtsam {
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
// -> SymbolicBayesNet -> SymbolicBayesTree
tic("JT 1.1 symbolic elimination");
SymbolicBayesNet::shared_ptr sbn = EliminationTree<IndexFactor>::Create(fg)->eliminate();
SymbolicBayesNet::shared_ptr sbn = SymbolicSequentialSolver(fg).eliminate();
// SymbolicFactorGraph sfg(fg);
// SymbolicBayesNet::shared_ptr sbn_orig = Inference::Eliminate(sfg);
// assert(assert_equal(*sbn, *sbn_orig));

2
inference/Makefile.am
View File

@ -18,7 +18,7 @@ check_PROGRAMS =
headers += Factor.h Factor-inl.h Conditional.h
# Symbolic Inference
sources += SymbolicFactorGraph.cpp
sources += SymbolicFactorGraph.cpp SymbolicSequentialSolver.cpp
check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional
check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots

2
inference/SymbolicFactorGraph.h
View File

@ -25,10 +25,12 @@
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/EliminationTree.h>
namespace gtsam {
typedef BayesNet<IndexConditional> SymbolicBayesNet;
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
/** Symbolic IndexFactor Graph */
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {

48
inference/VariableIndex.h
View File

@ -25,6 +25,7 @@
#include <deque>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
namespace gtsam {
@ -72,6 +73,7 @@ protected:
public:
VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph, Index nVariables);
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
Index size() const { return index_.size(); }
@ -90,6 +92,8 @@ protected:
VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
void checkVar(Index variable) const { assert(variable < index_.size()); }
template<class FactorGraph> void fill(const FactorGraph& factorGraph);
factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
const_factor_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
@ -115,6 +119,27 @@ void VariableIndex<Storage>::permute(const Permutation& permutation) {
// this->index_[j].swap(original[permutation[j]]);
}
/* ************************************************************************* */
template<class Storage>
template<class FactorGraph>
void VariableIndex<Storage>::fill(const FactorGraph& factorGraph) {
// Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
if(key < index_.size()) {
index_[key].push_back(mapped_factor_type(fi, fvari));
++ fvari;
++ nEntries_;
}
}
++ nFactors_;
}
}
/* ************************************************************************* */
template<class Storage>
template<class FactorGraph>
@ -135,22 +160,21 @@ VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
}
}
// Allocate index
// Allocate array
index_.container().resize(maxVar+1);
index_.permutation() = Permutation::Identity(maxVar+1);
// Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
index_[key].push_back(mapped_factor_type(fi, fvari));
++ fvari;
++ nEntries_;
}
++ nFactors_;
}
fill(factorGraph);
}
}
/* ************************************************************************* */
template<class Storage>
template<class FactorGraph>
VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph, Index nVariables) :
indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
fill(factorGraph);
}
/* ************************************************************************* */

395
inference/inference-inl.h
View File

@ -39,296 +39,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class FACTORGRAPH>
inline typename FACTORGRAPH::bayesnet_type::shared_ptr Inference::Eliminate(const FACTORGRAPH& factorGraph) {
// Create a copy of the factor graph to eliminate in-place
FACTORGRAPH eliminationGraph(factorGraph);
typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph);
return Eliminate(eliminationGraph, variableIndex);
}
/* ************************************************************************* */
//template<class FACTOR>
//BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph) {
//
// // Create a copy of the factor graph to eliminate in-place
// FactorGraph<gtsam::Factor> eliminationGraph(factorGraph);
// VariableIndex<> variableIndex(eliminationGraph);
//
// typename BayesNet<Conditional>::shared_ptr bayesnet(new BayesNet<Conditional>());
//
// // Eliminate variables one-by-one, updating the eliminated factor graph and
// // the variable index.
// for(Index var = 0; var < variableIndex.size(); ++var) {
// Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var));
// if(conditional) // Will be NULL if the variable did not appear in the factor graph.
// bayesnet->push_back(conditional);
// }
//
// return bayesnet;
//}
/* ************************************************************************* */
template<class FACTORGRAPH>
inline typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::Eliminate(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex) {
return EliminateUntil(factorGraph, variableIndex.size(), variableIndex);
}
/* ************************************************************************* */
template<class FACTORGRAPH>
inline typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::EliminateUntil(const FACTORGRAPH& factorGraph, Index bound) {
// Create a copy of the factor graph to eliminate in-place
FACTORGRAPH eliminationGraph(factorGraph);
typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph);
return EliminateUntil(eliminationGraph, bound, variableIndex);
}
/* ************************************************************************* */
template<class FACTORGRAPH>
typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex) {
typename FACTORGRAPH::bayesnet_type::shared_ptr bayesnet(new typename FACTORGRAPH::bayesnet_type);
// Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index.
for(Index var = 0; var < bound; ++var) {
typename FACTORGRAPH::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var));
if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional);
}
return bayesnet;
}
/* ************************************************************************* */
template<class FACTORGRAPH>
typename FACTORGRAPH::bayesnet_type::sharedConditional
Inference::EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var) {
/* This function performs symbolic elimination of a variable, comprising
* combining involved factors (analogous to "assembly" in SPQR) followed by
* eliminating to an upper-trapezoidal factor using spqr_front. This
* function performs the bookkeeping necessary for high performance.
*
* When combining factors, variables are merge sorted so that they remain
* in elimination order in the combined factor. GaussianFactor combines
* rows such that the row index after the last structural non-zero in each
* column increases monotonically (referred to as the "staircase" pattern in
* SPQR). The variable ordering is passed into the factor's Combine(...)
* function, which does the work of actually building the combined factor
* (for a GaussianFactor this assembles the augmented matrix).
*
* Next, this function calls the factor's eliminateFirst() function, which
* factorizes the factor into a conditional on the first variable and a
* factor on the remaining variables. In addition, this function updates the
* bookkeeping of the pattern of structural non-zeros. The GaussianFactor
* calls spqr_front during eliminateFirst(), which reduces its matrix to
* upper-trapezoidal form.
*
* Returns NULL if the variable does not appear in factorGraph.
*/
tic("EliminateOne");
// Get the factors involving the eliminated variable
typename FACTORGRAPH::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]);
typedef typename FACTORGRAPH::variableindex_type::mapped_factor_type mapped_factor_type;
if(!varIndexEntry.empty()) {
vector<size_t> removedFactors(varIndexEntry.size());
transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(),
boost::lambda::bind(&FACTORGRAPH::variableindex_type::mapped_factor_type::factorIndex, boost::lambda::_1));
// The new joint factor will be the last one in the factor graph
size_t jointFactorIndex = factorGraph.size();
static const bool debug = false;
if(debug) {
cout << "Eliminating " << var;
factorGraph.print(" from graph: ");
cout << removedFactors.size() << " factors to remove" << endl;
}
// Compute the involved keys, uses the variableIndex to mark whether each
// key has been added yet, but the positions stored in the variableIndex are
// from the unsorted positions and will be fixed later.
tic("EliminateOne: Find involved vars");
map<Index, size_t, std::less<Index>, boost::fast_pool_allocator<pair<const Index,size_t> > > involvedKeys; // Variable and original order as discovered
BOOST_FOREACH(size_t removedFactorI, removedFactors) {
if(debug) cout << removedFactorI << " is involved" << endl;
// If the factor has not previously been removed
if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
// Loop over the variables involved in the removed factor to update the
// variable index and joint factor positions of each variable.
BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
// Mark the new joint factor as involving each variable in the removed factor.
assert(!variableIndex[involvedVariable].empty());
if(variableIndex[involvedVariable].back().factorIndex != jointFactorIndex) {
if(debug) cout << " pulls in variable " << involvedVariable << endl;
size_t varpos = involvedKeys.size();
variableIndex[involvedVariable].push_back(mapped_factor_type(jointFactorIndex, varpos));
#ifndef NDEBUG
bool inserted =
#endif
involvedKeys.insert(make_pair(involvedVariable, varpos)).second;
assert(inserted);
} else if(debug)
cout << " involves variable " << involvedVariable << " which was previously discovered" << endl;
}
}
}
toc("EliminateOne: Find involved vars");
if(debug) cout << removedFactors.size() << " factors to remove" << endl;
// Compute the permutation to go from the original varpos to the sorted
// joint factor varpos
if(debug) cout << "Sorted keys:";
tic("EliminateOne: Sort involved vars");
vector<size_t> varposPermutation(involvedKeys.size(), numeric_limits<size_t>::max());
vector<Index> sortedKeys(involvedKeys.size());
{
size_t sortedVarpos = 0;
const map<Index, size_t, std::less<Index>, boost::fast_pool_allocator<pair<const Index,size_t> > >& involvedKeysC(involvedKeys);
for(map<Index, size_t, std::less<Index>, boost::fast_pool_allocator<pair<const Index,size_t> > >::const_iterator key_pos=involvedKeysC.begin(); key_pos!=involvedKeysC.end(); ++key_pos) {
sortedKeys[sortedVarpos] = key_pos->first;
assert(varposPermutation[key_pos->second] == numeric_limits<size_t>::max());
varposPermutation[key_pos->second] = sortedVarpos;
if(debug) cout << " " << key_pos->first << " (" << key_pos->second << "->" << sortedVarpos << ") ";
++ sortedVarpos;
}
}
toc("EliminateOne: Sort involved vars");
if(debug) cout << endl;
assert(sortedKeys.front() == var);
if(debug) cout << removedFactors.size() << " factors to remove" << endl;
// Fix the variable positions in the variableIndex
tic("EliminateOne: Fix varIndex");
for(size_t sortedPos=0; sortedPos<sortedKeys.size(); ++sortedPos) {
Index var = sortedKeys[sortedPos];
assert(!variableIndex[var].empty());
assert(variableIndex[var].back().factorIndex == jointFactorIndex);
assert(sortedPos == varposPermutation[variableIndex[var].back().variablePosition]);
if(debug) cout << "Fixing " << var << " " << variableIndex[var].back().variablePosition << "->" << sortedPos << endl;
variableIndex[var].back().variablePosition = sortedPos;
}
toc("EliminateOne: Fix varIndex");
// Fill in the jointFactorPositions
tic("EliminateOne: Fill jointFactorPositions");
vector<size_t> removedFactorIdxs;
removedFactorIdxs.reserve(removedFactors.size());
vector<vector<size_t> > jointFactorPositions;
jointFactorPositions.reserve(removedFactors.size());
if(debug) cout << removedFactors.size() << " factors to remove" << endl;
BOOST_FOREACH(size_t removedFactorI, removedFactors) {
if(debug) cout << "Fixing variable positions for factor " << removedFactorI << endl;
// If the factor has not previously been removed
if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
// Allocate space
jointFactorPositions.push_back(vector<size_t>());
vector<size_t>& jointFactorPositionsCur(jointFactorPositions.back());
jointFactorPositionsCur.reserve(factorGraph[removedFactorI]->keys().size());
removedFactorIdxs.push_back(removedFactorI);
// Loop over the variables involved in the removed factor to update the
// variable index and joint factor positions of each variable.
BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
// Mark the new joint factor as involving each variable in the removed factor
assert(!variableIndex[involvedVariable].empty());
assert(variableIndex[involvedVariable].back().factorIndex == jointFactorIndex);
const size_t varpos = variableIndex[involvedVariable].back().variablePosition;
jointFactorPositionsCur.push_back(varpos);
if(debug) cout << "Variable " << involvedVariable << " from factor " << removedFactorI;
if(debug) cout << " goes in position " << varpos << " of the joint factor" << endl;
assert(sortedKeys[varpos] == involvedVariable);
}
}
}
toc("EliminateOne: Fill jointFactorPositions");
// Join the factors and eliminate the variable from the joint factor
tic("EliminateOne: Combine");
typename FACTORGRAPH::sharedFactor jointFactor(
FACTORGRAPH::Factor::Combine(
factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions));
toc("EliminateOne: Combine");
// Remove the original factors
BOOST_FOREACH(size_t removedFactorI, removedFactors) {
if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI])
factorGraph.remove(removedFactorI);
}
typename FACTORGRAPH::bayesnet_type::sharedConditional conditional;
tic("EliminateOne: eliminateFirst");
conditional = jointFactor->eliminateFirst(); // Eliminate the first variable in-place
toc("EliminateOne: eliminateFirst");
tic("EliminateOne: store eliminated");
variableIndex[sortedKeys.front()].pop_back(); // Unmark the joint factor from involving the eliminated variable
factorGraph.push_back(jointFactor); // Put the eliminated factor into the factor graph
toc("EliminateOne: store eliminated");
toc("EliminateOne");
return conditional;
} else { // varIndexEntry.empty()
toc("EliminateOne");
return typename FACTORGRAPH::bayesnet_type::sharedConditional();
}
}
/* ************************************************************************* */
template<class FACTORGRAPH, class VARCONTAINER>
FACTORGRAPH Inference::Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables) {
// Compute a COLAMD permutation with the marginal variables constrained to the end
typename FACTORGRAPH::variableindex_type varIndex(factorGraph);
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(varIndex, variables));
Permutation::shared_ptr permutationInverse(permutation->inverse());
// Copy and permute the factors
varIndex.permute(*permutation);
FACTORGRAPH eliminationGraph; eliminationGraph.reserve(factorGraph.size());
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, factorGraph) {
typename FACTORGRAPH::sharedFactor permFactor(new typename FACTORGRAPH::Factor(*factor));
permFactor->permuteWithInverse(*permutationInverse);
eliminationGraph.push_back(permFactor);
}
// Eliminate all variables
typename FACTORGRAPH::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex));
// The last conditionals in the eliminated BayesNet contain the marginal for
// the variables we want. Undo the permutation as we add the marginal
// factors.
FACTORGRAPH marginal; marginal.reserve(variables.size());
typename FACTORGRAPH::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
for(Index j=0; j<variables.size(); ++j, ++conditional) {
typename FACTORGRAPH::sharedFactor factor(new typename FACTORGRAPH::Factor(**conditional));
factor->permuteWithInverse(*permutation);
marginal.push_back(factor);
assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end());
}
// Undo the permutation
return marginal;
}
/* ************************************************************************* */
template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) {
@ -398,109 +108,4 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& va
return permutation;
}
// /* ************************************************************************* */
// /* eliminate one node from the factor graph */
// /* ************************************************************************* */
// template<class Factor,class Conditional>
// boost::shared_ptr<Conditional> eliminateOne(FactorGraph<Factor>& graph, Index key) {
//
// // combine the factors of all nodes connected to the variable to be eliminated
// // if no factors are connected to key, returns an empty factor
// boost::shared_ptr<Factor> joint_factor = removeAndCombineFactors(graph,key);
//
// // eliminate that joint factor
// boost::shared_ptr<Factor> factor;
// boost::shared_ptr<Conditional> conditional;
// boost::tie(conditional, factor) = joint_factor->eliminate(key);
//
// // add new factor on separator back into the graph
// if (!factor->empty()) graph.push_back(factor);
//
// // return the conditional Gaussian
// return conditional;
// }
//
// /* ************************************************************************* */
// // This doubly templated function is generic. There is a GaussianFactorGraph
// // version that returns a more specific GaussianBayesNet.
// // Note, you will need to include this file to instantiate the function.
// /* ************************************************************************* */
// template<class Factor,class Conditional>
// BayesNet<Conditional> eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering)
// {
// BayesNet<Conditional> bayesNet; // empty
//
// BOOST_FOREACH(Index key, ordering) {
// boost::shared_ptr<Conditional> cg = eliminateOne<Factor,Conditional>(factorGraph,key);
// bayesNet.push_back(cg);
// }
//
// return bayesNet;
// }
// /* ************************************************************************* */
// template<class Factor, class Conditional>
// pair< BayesNet<Conditional>, FactorGraph<Factor> >
// factor(const BayesNet<Conditional>& bn, const Ordering& keys) {
// // Convert to factor graph
// FactorGraph<Factor> factorGraph(bn);
//
// // Get the keys of all variables and remove all keys we want the marginal for
// Ordering ord = bn.ordering();
// BOOST_FOREACH(Index key, keys) ord.remove(key); // TODO: O(n*k), faster possible?
//
// // eliminate partially,
// BayesNet<Conditional> conditional = eliminate<Factor,Conditional>(factorGraph,ord);
//
// // at this moment, the factor graph only encodes P(keys)
// return make_pair(conditional,factorGraph);
// }
//
// /* ************************************************************************* */
// template<class Factor, class Conditional>
// FactorGraph<Factor> marginalize(const BayesNet<Conditional>& bn, const Ordering& keys) {
//
// // factor P(X,Y) as P(X|Y)P(Y), where Y corresponds to keys
// pair< BayesNet<Conditional>, FactorGraph<Factor> > factors =
// gtsam::factor<Factor,Conditional>(bn,keys);
//
// // throw away conditional, return marginal P(Y)
// return factors.second;
// }
/* ************************************************************************* */
// pair<Vector,Matrix> marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key) {
//
// // todo: this does not use colamd!
//
// list<Symbol> ord;
// BOOST_FOREACH(const Symbol& k, fg.keys()) {
// if(k != key)
// ord.push_back(k);
// }
// Ordering ordering(ord);
//
// // Now make another factor graph where we eliminate all the other variables
// GaussianFactorGraph marginal(fg);
// marginal.eliminate(ordering);
//
// GaussianFactor::shared_ptr factor;
// for(size_t i=0; i<marginal.size(); i++)
// if(marginal[i] != NULL) {
// factor = marginal[i];
// break;
// }
//
// if(factor->keys().size() != 1 || factor->keys().front() != key)
// throw runtime_error("Didn't get the right marginal!");
//
// VectorValues mean_cfg(marginal.optimize(Ordering(key)));
// Matrix A(factor->get_A(key));
//
// return make_pair(mean_cfg[key], inverse(prod(trans(A), A)));
// }
/* ************************************************************************* */
} // namespace gtsam

79
inference/inference.cpp
View File

@ -22,83 +22,4 @@
namespace gtsam {
/* ************************************************************************* */
//Conditional::shared_ptr
//Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var) {
//
// tic("EliminateOne");
//
// // Get the factors involving the eliminated variable
// VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]);
// typedef VariableIndex<>::mapped_factor_type mapped_factor_type;
//
// if(!varIndexEntry.empty()) {
//
// vector<size_t> removedFactors(varIndexEntry.size());
// transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(),
// boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1));
//
// // The new joint factor will be the last one in the factor graph
// size_t jointFactorIndex = factorGraph.size();
//
// static const bool debug = false;
//
// if(debug) {
// cout << "Eliminating " << var;
// factorGraph.print(" from graph: ");
// cout << removedFactors.size() << " factors to remove" << endl;
// }
//
// // Compute the involved keys, uses the variableIndex to mark whether each
// // key has been added yet, but the positions stored in the variableIndex are
// // from the unsorted positions and will be fixed later.
// tic("EliminateOne: Find involved vars");
// typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > InvolvedKeys;
// InvolvedKeys involvedKeys;
// BOOST_FOREACH(size_t removedFactorI, removedFactors) {
// if(debug) cout << removedFactorI << " is involved" << endl;
// // If the factor has not previously been removed
// if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
// // Loop over the variables involved in the removed factor to update the
// // variable index and joint factor positions of each variable.
// BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
// if(debug) cout << " pulls in variable " << involvedVariable << endl;
// // Mark the new joint factor as involving each variable in the removed factor.
// assert(!variableIndex[involvedVariable].empty());
// involvedKeys.insert(involvedVariable);
// }
// }
//
// // Remove the original factor
// factorGraph.remove(removedFactorI);
// }
//
// // We need only mark the next variable to be eliminated as involved with the joint factor
// if(involvedKeys.size() > 1) {
// InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next;
// variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0));
// }
// toc("EliminateOne: Find involved vars");
// if(debug) cout << removedFactors.size() << " factors to remove" << endl;
//
// // Join the factors and eliminate the variable from the joint factor
// tic("EliminateOne: Combine");
// Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1);
// Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents()));
// toc("EliminateOne: Combine");
//
// tic("EliminateOne: store eliminated");
// factorGraph.push_back(eliminated); // Put the eliminated factor into the factor graph
// toc("EliminateOne: store eliminated");
//
// toc("EliminateOne");
//
// return conditional;
//
// } else { // varIndexEntry.empty()
// toc("EliminateOne");
// return Conditional::shared_ptr();
// }
//}
}

78
inference/inference.h
View File

@ -36,74 +36,6 @@ namespace gtsam {
public:
/**
* Eliminate a factor graph in its natural ordering, i.e. eliminating
* variables in order starting from 0.
*/
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(const FACTORGRAPH& factorGraph);
/**
* Eliminate a factor graph in its natural ordering, i.e. eliminating
* variables in order starting from 0. Special fast version for symbolic
* elimination.
*/
// template<class FACTOR>
// static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph);
/**
* Eliminate a factor graph in its natural ordering, i.e. eliminating
* variables in order starting from 0. Uses an existing
* variable index instead of building one from scratch.
*/
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(
FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex);
/**
* Partially eliminate a factor graph, up to but not including the given
* variable.
*/
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr
EliminateUntil(const FACTORGRAPH& factorGraph, Index bound);
/**
* Partially eliminate a factor graph, up to but not including the given
* variable. Use an existing variable index instead of building one from
* scratch.
*/
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr
EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex);
/**
* Eliminate a single variable, updating an existing factor graph and
* variable index.
*/
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::sharedConditional
EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var);
/**
* Eliminate a single variable, updating an existing factor graph and
* variable index. This is a specialized faster version for purely
* symbolic factor graphs.
*/
// static boost::shared_ptr<Conditional>
// EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var);
/**
* Eliminate all variables except the specified ones. Internally this
* permutes these variables to the end of the ordering, eliminates all
* other variables, and then undoes the permutation. This is
* inefficient if multiple marginals are needed - in that case use the
* BayesTree which supports efficiently computing marginals for multiple
* variables.
*/
template<class FACTORGRAPH, class VARCONTAINER>
static FACTORGRAPH Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables);
/**
* Compute a permutation (variable ordering) using colamd
*/
@ -112,16 +44,6 @@ namespace gtsam {
template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast);
// /**
// * Join several factors into one. This involves determining the set of
// * shared variables and the correct variable positions in the new joint
// * factor.
// */
// template<class FACTORGRAPH, typename InputIterator>
// static typename FACTORGRAPH::shared_factor Combine(const FACTORGRAPH& factorGraph,
// InputIterator indicesBegin, InputIterator indicesEnd);
};
// ELIMINATE: FACTOR GRAPH -> BAYES NET

113
inference/tests/testBayesTree.cpp
View File

@ -26,7 +26,7 @@ using namespace boost::assign;
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
using namespace gtsam;
@ -333,62 +333,61 @@ TEST( BayesTree, removeTop3 )
CHECK(orphans.size() == 0);
}
/* ************************************************************************* */
/**
* x2 - x3 - x4 - x5
* | / \ |
* x1 / \ x6
*/
TEST( BayesTree, insert )
{
// construct bayes tree by split the graph along the separator x3 - x4
const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
SymbolicFactorGraph fg1, fg2, fg3;
fg1.push_factor(_x3_, _x4_);
fg2.push_factor(_x1_, _x2_);
fg2.push_factor(_x2_, _x3_);
fg2.push_factor(_x1_, _x3_);
fg3.push_factor(_x5_, _x4_);
fg3.push_factor(_x6_, _x5_);
fg3.push_factor(_x6_, _x4_);
// Ordering ordering1; ordering1 += _x3_, _x4_;
// Ordering ordering2; ordering2 += _x1_, _x2_;
// Ordering ordering3; ordering3 += _x6_, _x5_;
BayesNet<IndexConditional> bn1, bn2, bn3;
bn1 = *Inference::EliminateUntil(fg1, _x4_+1);
bn2 = *Inference::EliminateUntil(fg2, _x2_+1);
bn3 = *Inference::EliminateUntil(fg3, _x5_+1);
// insert child cliques
SymbolicBayesTree actual;
list<SymbolicBayesTree::sharedClique> children;
SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children);
SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children);
// insert root clique
children.push_back(r1);
children.push_back(r2);
actual.insert(bn1, children, true);
// traditional way
SymbolicFactorGraph fg;
fg.push_factor(_x3_, _x4_);
fg.push_factor(_x1_, _x2_);
fg.push_factor(_x2_, _x3_);
fg.push_factor(_x1_, _x3_);
fg.push_factor(_x5_, _x4_);
fg.push_factor(_x6_, _x5_);
fg.push_factor(_x6_, _x4_);
// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
BayesNet<IndexConditional> bn;
bn = *Inference::Eliminate(fg);
SymbolicBayesTree expected(bn);
CHECK(assert_equal(expected, actual));
}
///* ************************************************************************* */
///**
// * x2 - x3 - x4 - x5
// * | / \ |
// * x1 / \ x6
// */
//TEST( BayesTree, insert )
//{
// // construct bayes tree by split the graph along the separator x3 - x4
// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
// SymbolicFactorGraph fg1, fg2, fg3;
// fg1.push_factor(_x3_, _x4_);
// fg2.push_factor(_x1_, _x2_);
// fg2.push_factor(_x2_, _x3_);
// fg2.push_factor(_x1_, _x3_);
// fg3.push_factor(_x5_, _x4_);
// fg3.push_factor(_x6_, _x5_);
// fg3.push_factor(_x6_, _x4_);
//
//// Ordering ordering1; ordering1 += _x3_, _x4_;
//// Ordering ordering2; ordering2 += _x1_, _x2_;
//// Ordering ordering3; ordering3 += _x6_, _x5_;
//
// BayesNet<IndexConditional> bn1, bn2, bn3;
// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1);
// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1);
// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1);
//
// // insert child cliques
// SymbolicBayesTree actual;
// list<SymbolicBayesTree::sharedClique> children;
// SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children);
// SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children);
//
// // insert root clique
// children.push_back(r1);
// children.push_back(r2);
// actual.insert(bn1, children, true);
//
// // traditional way
// SymbolicFactorGraph fg;
// fg.push_factor(_x3_, _x4_);
// fg.push_factor(_x1_, _x2_);
// fg.push_factor(_x2_, _x3_);
// fg.push_factor(_x1_, _x3_);
// fg.push_factor(_x5_, _x4_);
// fg.push_factor(_x6_, _x5_);
// fg.push_factor(_x6_, _x4_);
//
//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
// BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
// SymbolicBayesTree expected(bn);
// CHECK(assert_equal(expected, actual));
//
//}
/* ************************************************************************* */
int main() {

8
inference/tests/testEliminationTree.cpp
View File

@ -9,13 +9,11 @@
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
using namespace gtsam;
using namespace std;
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
struct EliminationTreeTester {
// build hardcoded tree
static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) {
@ -76,10 +74,10 @@ TEST(EliminationTree, eliminate )
IndexConditional::shared_ptr c4(new IndexConditional(4));
SymbolicBayesNet expected;
expected.push_back(c3);
expected.push_back(c0);
expected.push_back(c1);
expected.push_back(c2);
expected.push_back(c3);
expected.push_back(c4);
// Create factor graph
@ -91,7 +89,7 @@ TEST(EliminationTree, eliminate )
fg.push_factor(3, 4);
// eliminate
SymbolicBayesNet actual = *SymbolicEliminationTree::Create(fg)->eliminate();
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected,actual));
}

2
inference/tests/testJunctionTree.cpp
View File

@ -83,7 +83,7 @@ TEST( JunctionTree, eliminate)
SymbolicJunctionTree jt(fg);
SymbolicBayesTree::sharedClique actual = jt.eliminate();
BayesNet<IndexConditional> bn = *Inference::Eliminate(fg);
BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
SymbolicBayesTree expected(bn);
// cout << "BT from JT:\n";

31
inference/tests/testSymbolicFactorGraph.cpp
View File

@ -24,7 +24,7 @@ using namespace boost::assign;
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
using namespace std;
using namespace gtsam;
@ -33,21 +33,20 @@ static const Index vx2 = 0;
static const Index vx1 = 1;
static const Index vl1 = 2;
/* ************************************************************************* */
TEST( SymbolicFactorGraph, EliminateOne )
{
// create a test graph
SymbolicFactorGraph fg;
fg.push_factor(vx2, vx1);
VariableIndex<> variableIndex(fg);
Inference::EliminateOne(fg, variableIndex, vx2);
SymbolicFactorGraph expected;
expected.push_back(boost::shared_ptr<IndexFactor>());
expected.push_factor(vx1);
CHECK(assert_equal(expected, fg));
}
///* ************************************************************************* */
//TEST( SymbolicFactorGraph, EliminateOne )
//{
// // create a test graph
// SymbolicFactorGraph fg;
// fg.push_factor(vx2, vx1);
//
// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1);
// SymbolicFactorGraph expected;
// expected.push_back(boost::shared_ptr<IndexFactor>());
// expected.push_factor(vx1);
//
// CHECK(assert_equal(expected, fg));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, constructFromBayesNet )

3
linear/Factorization.h
View File

@ -22,6 +22,7 @@
#include <stdexcept>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/inference-inl.h>
namespace gtsam {
@ -47,7 +48,7 @@ namespace gtsam {
* the resulted linear system
*/
VectorValues optimize(GaussianFactorGraph& fg) const {
return gtsam::optimize(*Inference::Eliminate(fg));
return *GaussianSequentialSolver(fg).optimize();
}
/**

6
linear/GaussianFactorGraph.h
View File

@ -51,6 +51,12 @@ namespace gtsam {
*/
GaussianFactorGraph(const GaussianBayesNet& CBN);
/** Constructor from a factor graph of GaussianFactor or a derived type */
template<class DERIVEDFACTOR>
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) {
push_back(fg);
}
/** Add a null factor */
void add(const Vector& b) {
push_back(sharedFactor(new GaussianFactor(b)));

3
linear/Makefile.am
View File

@ -21,6 +21,9 @@ check_PROGRAMS += tests/testVectorValues
#sources += VectorMap.cpp VectorBTree.cpp
#check_PROGRAMS += tests/testVectorMap tests/testVectorBTree
# Solvers
sources += GaussianSequentialSolver.cpp
# Gaussian Factor Graphs
headers += GaussianFactorSet.h Factorization.h
sources += GaussianFactor.cpp GaussianFactorGraph.cpp

3
linear/SubgraphSolver-inl.h
View File

@ -24,6 +24,7 @@
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
using namespace std;
@ -69,7 +70,7 @@ namespace gtsam {
SubgraphPreconditioner::sharedValues xbar;
#else
GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = Inference::Eliminate(sacrificialAb1) ;
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
#endif
// TODO: there does not seem to be a good reason to have Ab1_

33
linear/tests/testGaussianFactor.cpp
View File

@ -34,7 +34,7 @@ using namespace boost::assign;
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
using namespace std;
using namespace gtsam;
@ -610,22 +610,21 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
CHECK(assert_equal(expectedLF,actualLF,1e-5));
}
/* ************************************************************************* */
TEST( GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained )
{
Matrix Ax = eye(2);
Vector b = Vector_(2, 3.0, 5.0);
SharedDiagonal noisemodel = noiseModel::Constrained::All(2);
GaussianFactor::shared_ptr expected(new GaussianFactor(_x0_, Ax, b, noisemodel));
GaussianFactorGraph graph;
graph.push_back(expected);
GaussianVariableIndex<> index(graph);
GaussianConditional::shared_ptr conditional = Inference::EliminateOne(graph,index,_x0_);
GaussianFactor actual(*conditional);
CHECK(assert_equal(*expected, actual));
}
///* ************************************************************************* */
//TEST( GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained )
//{
// Matrix Ax = eye(2);
// Vector b = Vector_(2, 3.0, 5.0);
// SharedDiagonal noisemodel = noiseModel::Constrained::All(2);
// GaussianFactor::shared_ptr expected(new GaussianFactor(_x0_, Ax, b, noisemodel));
// GaussianFactorGraph graph;
// graph.push_back(expected);
//
// GaussianConditional::shared_ptr conditional = GaussianSequentialSolver::EliminateUntil(graph,_x0_+1);
// GaussianFactor actual(*conditional);
//
// CHECK(assert_equal(*expected, actual));
//}
/* ************************************************************************* */
TEST ( GaussianFactor, constraint_eliminate1 )

7
linear/tests/timeFactorOverhead.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <boost/random.hpp>
#include <boost/timer.hpp>
@ -27,6 +28,8 @@
using namespace gtsam;
using namespace std;
typedef EliminationTree<GaussianFactor> GaussianEliminationTree;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
int main(int argc, char *argv[]) {
@ -80,7 +83,7 @@ int main(int argc, char *argv[]) {
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial]));
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate());
VectorValues soln(optimize(*gbn));
}
blocksolve = timer.elapsed();
@ -124,7 +127,7 @@ int main(int argc, char *argv[]) {
cout.flush();
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(combGfgs[trial]));
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate());
VectorValues soln(optimize(*gbn));
}
combsolve = timer.elapsed();

5
linear/tests/timeSLAMlike.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <boost/random.hpp>
#include <boost/timer.hpp>
@ -30,6 +31,8 @@ using namespace gtsam;
using namespace std;
using namespace boost::lambda;
typedef EliminationTree<GaussianFactor> GaussianEliminationTree;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
bool _pair_compare(const pair<Index, Matrix>& a1, const pair<Index, Matrix>& a2) { return a1.first < a2.first; }
@ -116,7 +119,7 @@ int main(int argc, char *argv[]) {
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial]));
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate());
VectorValues soln(optimize(*gbn));
}
blocksolve = timer.elapsed();

2
myconfigure
View File

@ -1,4 +1,4 @@
../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPPFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install
../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" CPPFLAGS="-D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install
#../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CXXFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install
#cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -DNDEBUG -O3" CFLAGS="-fno-inline -g -DNDEBUG -O3" LDFLAGS="-fno-inline -g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
#cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-DNDEBUG -O3" CFLAGS="-g -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack

2
myconfigure.timing
View File

@ -1,2 +1,2 @@
../configure --prefix=$HOME/borg --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack
#../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CXXFLAGS="-DNDEBUG -g" CFLAGS="-DNDEBUG -g" LDFLAGS="-DNDEBUG -g" --disable-static --enable-blas --enable-lapack

5
tests/testGaussianBayesNet.cpp
View File

@ -34,7 +34,7 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/slam/smallExample.h>
using namespace std;
@ -114,8 +114,7 @@ TEST( GaussianBayesNet, optimize2 )
fg.add(_x_, -eye(1), _z_, eye(1), 2*ones(1), noise);
GaussianBayesNet cbn = *Inference::Eliminate(fg);
VectorValues actual = optimize(cbn);
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
VectorValues expected(vector<size_t>(3,1));
expected[_x_] = Vector_(1,1.);

122
tests/testGaussianFactorGraph.cpp
View File

@ -36,8 +36,7 @@ using namespace boost::assign;
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
//#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
using namespace gtsam;
using namespace example;
@ -195,55 +194,52 @@ TEST( GaussianFactorGraph, error )
// CHECK(assert_equal(expected,*actual));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1 )
{
Ordering ordering; ordering += "x1","l1","x2";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianVariableIndex<> varindex(fg);
GaussianConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, 0);
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected(ordering["x1"],15*d,R11,ordering["l1"],S12,ordering["x2"],S13,sigma);
CHECK(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2 )
{
Ordering ordering; ordering += "x2","l1","x1";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianVariableIndex<> varindex(fg);
GaussianConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, 0);
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering["x2"],d,R11,ordering["l1"],S12,ordering["x1"],S13,sigma);
CHECK(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1 )
{
Ordering ordering; ordering += "l1","x1","x2";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianVariableIndex<> varindex(fg);
GaussianConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, 0);
// create expected Conditional Gaussian
double sig = sqrt(2)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering["l1"],d,R11,ordering["x1"],S12,ordering["x2"],S13,sigma);
CHECK(assert_equal(expected,*actual,tol));
}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x1 )
//{
// Ordering ordering; ordering += "x1","l1","x2";
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
//
// // create expected Conditional Gaussian
// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
// GaussianConditional expected(ordering["x1"],15*d,R11,ordering["l1"],S12,ordering["x2"],S13,sigma);
//
// CHECK(assert_equal(expected,*actual,tol));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x2 )
//{
// Ordering ordering; ordering += "x2","l1","x1";
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
//
// // create expected Conditional Gaussian
// double sig = 0.0894427;
// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
// GaussianConditional expected(ordering["x2"],d,R11,ordering["l1"],S12,ordering["x1"],S13,sigma);
//
// CHECK(assert_equal(expected,*actual,tol));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_l1 )
//{
// Ordering ordering; ordering += "l1","x1","x2";
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
//
// // create expected Conditional Gaussian
// double sig = sqrt(2)/10.;
// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
// GaussianConditional expected(ordering["l1"],d,R11,ordering["x1"],S12,ordering["x2"],S13,sigma);
//
// CHECK(assert_equal(expected,*actual,tol));
//}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x1_fast )
@ -311,10 +307,10 @@ TEST( GaussianFactorGraph, eliminateAll )
// Check one ordering
GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
GaussianBayesNet actual = *Inference::Eliminate(fg1);
GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
CHECK(assert_equal(expected,actual,tol));
GaussianBayesNet actualET = *EliminationTree<GaussianFactor>::Create(fg1)->eliminate();
GaussianBayesNet actualET = *GaussianSequentialSolver(fg1).eliminate();
CHECK(assert_equal(expected,actualET,tol));
}
@ -370,7 +366,7 @@ TEST( GaussianFactorGraph, copying )
GaussianFactorGraph copy = actual;
// now eliminate the copy
GaussianBayesNet actual1 = *Inference::Eliminate(copy);
GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate();
// Create the same graph, but not by copying
GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
@ -446,11 +442,11 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// render with a given ordering
GaussianBayesNet CBN = *Inference::Eliminate(fg);
GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate();
// True GaussianFactorGraph
GaussianFactorGraph fg2(CBN);
GaussianBayesNet CBN2 = *Inference::Eliminate(fg2);
GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
CHECK(assert_equal(CBN,CBN2));
// // Base FactorGraph only
@ -490,14 +486,12 @@ TEST( GaussianFactorGraph, optimize )
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// optimize the graph
VectorValues actual = optimize(*Inference::Eliminate(fg));
VectorValues actualET = optimize(*EliminationTree<GaussianFactor>::Create(fg)->eliminate());
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValues expected = createCorrectDelta(ord);
CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expected,actualET));
}
///* ************************************************************************* */
@ -759,7 +753,7 @@ TEST( GaussianFactorGraph, elimination )
fg.add(ord["x2"], Ap, b, sigma);
// Eliminate
GaussianBayesNet bayesNet = *Inference::Eliminate(fg);
GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
// Check sigma
DOUBLES_EQUAL(1.0,bayesNet[ord["x2"]]->get_sigmas()(0),1e-5);
@ -785,13 +779,11 @@ TEST( GaussianFactorGraph, constrained_simple )
GaussianFactorGraph fg = createSimpleConstraintGraph();
// eliminate and solve
VectorValues actual = optimize(*Inference::Eliminate(fg));
VectorValues actualET = optimize(*EliminationTree<GaussianFactor>::Create(fg)->eliminate());
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValues expected = createSimpleConstraintValues();
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(expected, actualET));
}
/* ************************************************************************* */
@ -801,7 +793,7 @@ TEST( GaussianFactorGraph, constrained_single )
GaussianFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve
VectorValues actual = optimize(*Inference::Eliminate(fg));
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValues expected = createSingleConstraintValues();
@ -831,7 +823,7 @@ TEST( GaussianFactorGraph, constrained_multi1 )
GaussianFactorGraph fg = createMultiConstraintGraph();
// eliminate and solve
VectorValues actual = optimize(*Inference::Eliminate(fg));
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValues expected = createMultiConstraintValues();

89
tests/testGaussianISAM.cpp
View File

@ -27,6 +27,7 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/slam/smallExample.h>
using namespace std;
@ -59,7 +60,7 @@ TEST( ISAM, iSAM_smoother )
}
// Create expected Bayes Tree by solving smoother with "natural" ordering
GaussianISAM expected(*Inference::Eliminate(smoother));
GaussianISAM expected(*GaussianSequentialSolver(smoother).eliminate());
// Check whether BayesTree is correct
CHECK(assert_equal(expected, actual));
@ -113,7 +114,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
boost::tie(smoother, ordering) = createSmoother(7);
// eliminate using the "natural" ordering
GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother);
GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
// Create the Bayes tree
GaussianISAM bayesTree(chordalBayesNet);
@ -122,12 +123,12 @@ TEST( BayesTree, linear_smoother_shortcuts )
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
GaussianISAM::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual1 = R->shortcut(R);
CHECK(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = bayesTree[ordering["x5"]];
GaussianBayesNet actual2 = C2->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual2 = C2->shortcut(R);
CHECK(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root)
@ -136,7 +137,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected3;
push_front(expected3,ordering["x5"], zero(2), eye(2)/sigma3, ordering["x6"], A56/sigma3, ones(2));
GaussianISAM::sharedClique C3 = bayesTree[ordering["x4"]];
GaussianBayesNet actual3 = C3->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual3 = C3->shortcut(R);
CHECK(assert_equal(expected3,actual3,tol));
// Check the conditional P(C4|Root)
@ -145,7 +146,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected4;
push_front(expected4, ordering["x4"], zero(2), eye(2)/sigma4, ordering["x6"], A46/sigma4, ones(2));
GaussianISAM::sharedClique C4 = bayesTree[ordering["x3"]];
GaussianBayesNet actual4 = C4->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual4 = C4->shortcut(R);
CHECK(assert_equal(expected4,actual4,tol));
}
@ -176,7 +177,7 @@ TEST( BayesTree, balanced_smoother_marginals )
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree
GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother);
GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
VectorValues expectedSolution(7, 2);
expectedSolution.makeZero();
@ -191,28 +192,28 @@ TEST( BayesTree, balanced_smoother_marginals )
// Check marginal on x1
GaussianBayesNet expected1 = simpleGaussian(ordering["x1"], zero(2), sigmax1);
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactorGraph>(ordering["x1"]);
GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering["x1"]);
CHECK(assert_equal(expected1,actual1,tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
GaussianBayesNet expected2 = simpleGaussian(ordering["x2"], zero(2), sigx2);
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactorGraph>(ordering["x2"]);
GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering["x2"]);
CHECK(assert_equal(expected2,actual2,tol)); // FAILS
// Check marginal on x3
GaussianBayesNet expected3 = simpleGaussian(ordering["x3"], zero(2), sigmax3);
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactorGraph>(ordering["x3"]);
GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering["x3"]);
CHECK(assert_equal(expected3,actual3,tol));
// Check marginal on x4
GaussianBayesNet expected4 = simpleGaussian(ordering["x4"], zero(2), sigmax4);
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactorGraph>(ordering["x4"]);
GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering["x4"]);
CHECK(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7 = simpleGaussian(ordering["x7"], zero(2), sigmax7);
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactorGraph>(ordering["x7"]);
GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering["x7"]);
CHECK(assert_equal(expected7,actual7,tol));
}
@ -225,56 +226,56 @@ TEST( BayesTree, balanced_smoother_shortcuts )
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree
GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother);
GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
GaussianISAM bayesTree(chordalBayesNet);
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
GaussianISAM::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual1 = R->shortcut(R);
CHECK(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = bayesTree[ordering["x3"]];
GaussianBayesNet actual2 = C2->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual2 = C2->shortcut(R);
CHECK(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet[ordering["x2"]];
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
GaussianISAM::sharedClique C3 = bayesTree[ordering["x1"]];
GaussianBayesNet actual3 = C3->shortcut<GaussianFactorGraph>(R);
GaussianBayesNet actual3 = C3->shortcut(R);
CHECK(assert_equal(expected3,actual3,tol));
}
/* ************************************************************************* */
TEST( BayesTree, balanced_smoother_clique_marginals )
{
// Create smoother with 7 nodes
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree
GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother);
GaussianISAM bayesTree(chordalBayesNet);
// Check the clique marginal P(C3)
double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
GaussianBayesNet expected = simpleGaussian(ordering["x2"],zero(2),sigmax2_alt);
push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2));
GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]];
GaussianFactorGraph marginal = C3->marginal<GaussianFactorGraph>(R);
GaussianVariableIndex<> varIndex(marginal);
Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
Permutation toFrontInverse(*toFront.inverse());
varIndex.permute(toFront);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
factor->permuteWithInverse(toFrontInverse); }
GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
actual.permuteWithInverse(toFront);
CHECK(assert_equal(expected,actual,tol));
}
///* ************************************************************************* */
//TEST( BayesTree, balanced_smoother_clique_marginals )
//{
// // Create smoother with 7 nodes
// Ordering ordering;
// ordering += "x1","x3","x5","x7","x2","x6","x4";
// GaussianFactorGraph smoother = createSmoother(7, ordering).first;
//
// // Create the Bayes tree
// GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
// GaussianISAM bayesTree(chordalBayesNet);
//
// // Check the clique marginal P(C3)
// double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
// GaussianBayesNet expected = simpleGaussian(ordering["x2"],zero(2),sigmax2_alt);
// push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2));
// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]];
// GaussianFactorGraph marginal = C3->marginal(R);
// GaussianVariableIndex<> varIndex(marginal);
// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
// Permutation toFrontInverse(*toFront.inverse());
// varIndex.permute(toFront);
// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
// factor->permuteWithInverse(toFrontInverse); }
// GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
// actual.permuteWithInverse(toFront);
// CHECK(assert_equal(expected,actual,tol));
//}
/* ************************************************************************* */
// SL-FIX TEST( BayesTree, balanced_smoother_joint )

21
tests/testGaussianJunctionTree.cpp
View File

@ -31,6 +31,7 @@ using namespace boost::assign;
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>
@ -69,15 +70,15 @@ TEST( GaussianJunctionTree, constructor2 )
list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
GaussianJunctionTree::sharedClique child0 = *child0it;
GaussianJunctionTree::sharedClique child1 = *child1it;
CHECK(assert_equal(frontal2, child1->frontal));
CHECK(assert_equal(sep2, child1->separator));
LONGS_EQUAL(4, child1->size());
CHECK(assert_equal(frontal3, child1->children().front()->frontal));
CHECK(assert_equal(sep3, child1->children().front()->separator));
LONGS_EQUAL(2, child1->children().front()->size());
CHECK(assert_equal(frontal4, child0->frontal));
CHECK(assert_equal(sep4, child0->separator));
LONGS_EQUAL(2, child0->size());
CHECK(assert_equal(frontal2, child0->frontal));
CHECK(assert_equal(sep2, child0->separator));
LONGS_EQUAL(4, child0->size());
CHECK(assert_equal(frontal3, child0->children().front()->frontal));
CHECK(assert_equal(sep3, child0->children().front()->separator));
LONGS_EQUAL(2, child0->children().front()->size());
CHECK(assert_equal(frontal4, child1->frontal));
CHECK(assert_equal(sep4, child1->separator));
LONGS_EQUAL(2, child1->size());
}
/* ************************************************************************* */
@ -176,7 +177,7 @@ TEST(GaussianJunctionTree, slamlike) {
VectorValues deltaactual = gjt.optimize();
planarSLAM::Values actual = init.expmap(deltaactual, ordering);
GaussianBayesNet gbn = *Inference::Eliminate(linearized);
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
VectorValues delta = optimize(gbn);
planarSLAM::Values expected = init.expmap(delta, ordering);

14
tests/testInference.cpp
View File

@ -20,7 +20,7 @@
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/smallExample.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
using namespace std;
using namespace gtsam;
@ -40,10 +40,10 @@ TEST(GaussianFactorGraph, createSmoother)
LONGS_EQUAL(5,fg2.size());
// eliminate
list<Index> x3var; x3var.push_back(ordering["x3"]);
list<Index> x1var; x1var.push_back(ordering["x1"]);
GaussianBayesNet p_x3 = *Inference::Eliminate(Inference::Marginal(fg2, x3var));
GaussianBayesNet p_x1 = *Inference::Eliminate(Inference::Marginal(fg2, x1var));
vector<Index> x3var; x3var.push_back(ordering["x3"]);
vector<Index> x1var; x1var.push_back(ordering["x1"]);
GaussianBayesNet p_x3 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x3var)).eliminate();
GaussianBayesNet p_x1 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x1var)).eliminate();
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
}
@ -52,8 +52,8 @@ TEST( Inference, marginals )
{
// create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet();
list<Index> xvar; xvar.push_back(0);
GaussianBayesNet actual = *Inference::Eliminate(Inference::Marginal(GaussianFactorGraph(cbn), xvar));
vector<Index> xvar; xvar.push_back(0);
GaussianBayesNet actual = *GaussianSequentialSolver(*GaussianSequentialSolver(GaussianFactorGraph(cbn)).joint(xvar)).eliminate();
// expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2));

4
tests/testSymbolicBayesNet.cpp
View File

@ -25,7 +25,7 @@ using namespace boost::assign;
#include <gtsam/slam/smallExample.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/nonlinear/Ordering.h>
using namespace std;
@ -56,7 +56,7 @@ TEST( SymbolicBayesNet, constructor )
SymbolicFactorGraph fg(factorGraph);
// eliminate it
SymbolicBayesNet actual = *Inference::Eliminate(fg);
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected, actual));
}

37
tests/testSymbolicFactorGraph.cpp
View File

@ -26,7 +26,7 @@ using namespace boost::assign;
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/nonlinear/Ordering.h>
using namespace std;
@ -106,23 +106,22 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
// CHECK(assert_equal(expected,*actual));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, eliminateOne )
{
Ordering o; o += "x1","l1","x2";
// create a test graph
GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
SymbolicFactorGraph fg(factorGraph);
// eliminate
VariableIndex<> varindex(fg);
IndexConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, o["x1"]);
// create expected symbolic IndexConditional
IndexConditional expected(o["x1"],o["l1"],o["x2"]);
CHECK(assert_equal(expected,*actual));
}
///* ************************************************************************* */
//TEST( SymbolicFactorGraph, eliminateOne )
//{
// Ordering o; o += "x1","l1","x2";
// // create a test graph
// GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
// SymbolicFactorGraph fg(factorGraph);
//
// // eliminate
// IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o["x1"]+1);
//
// // create expected symbolic IndexConditional
// IndexConditional expected(o["x1"],o["l1"],o["x2"]);
//
// CHECK(assert_equal(expected,*actual));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, eliminate )
@ -144,7 +143,7 @@ TEST( SymbolicFactorGraph, eliminate )
SymbolicFactorGraph fg(factorGraph);
// eliminate it
SymbolicBayesNet actual = *Inference::Eliminate(fg);
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected,actual));
}

8
tests/timeGaussianFactorGraph.cpp
View File

@ -22,7 +22,7 @@
#include <boost/assign/std/list.hpp> // for operator += in Ordering
#include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
using namespace std;
using namespace gtsam;
@ -35,7 +35,7 @@ double timeKalmanSmoother(int T) {
pair<GaussianFactorGraph,Ordering> smoother_ordering = createSmoother(T);
GaussianFactorGraph& smoother(smoother_ordering.first);
clock_t start = clock();
optimize(*Inference::Eliminate(smoother));
GaussianSequentialSolver(smoother).optimize();
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
@ -50,7 +50,7 @@ double timePlanarSmoother(int N, bool old = true) {
// Ordering& ordering(pg.get<1>());
// VectorValues& config(pg.get<2>());
clock_t start = clock();
optimize(*Inference::Eliminate(fg));
GaussianSequentialSolver(fg).optimize();
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
@ -65,7 +65,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
// Ordering& ordering(pg.get<1>());
// VectorValues& config(pg.get<2>());
clock_t start = clock();
Inference::Eliminate(fg);
GaussianSequentialSolver(fg).eliminate();
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;

14
tests/timeLinearOnDataset.cpp
View File

@ -35,22 +35,22 @@ int main(int argc, char *argv[]) {
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
tic("Z 1 order");
tic_("Z 1 order");
Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second));
toc("Z 1 order");
toc_("Z 1 order");
tic("Z 2 linearize");
tic_("Z 2 linearize");
GaussianFactorGraph::shared_ptr gfg(data.first->linearize(*data.second, *ordering));
toc("Z 2 linearize");
toc_("Z 2 linearize");
for(size_t trial = 0; trial < 100; ++trial) {
tic("Z 3 solve");
tic_("Z 3 solve");
GaussianJunctionTree gjt(*gfg);
VectorValues soln(gjt.optimize());
toc("Z 3 solve");
toc_("Z 3 solve");
tictoc_print();
tictoc_print_();
}
return 0;