fixed junction tree unit tests
Kai Ni
parent
925a5ca3c5
commit
2c5522f2fa
|
|
@ -48,8 +48,10 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
void BayesTree<Conditional>::Clique::print(const string& s) const {
|
||||
cout << s;
|
||||
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
|
||||
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) {
|
||||
conditional->print("conditioanl");
|
||||
cout << " " << (string)(conditional->key());
|
||||
}
|
||||
if (!separator_.empty()) {
|
||||
cout << " :";
|
||||
BOOST_FOREACH(const Symbol& key, separator_)
|
||||
|
|
@ -349,6 +351,34 @@ namespace gtsam {
|
|||
insert(*rit, index);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet, std::list<BayesTree<Conditional> > subtrees) {
|
||||
if (bayesNet.size() == 0)
|
||||
throw invalid_argument("BayesTree::insert: empty bayes net!");
|
||||
|
||||
// get the roots of child subtrees and merge their nodes_
|
||||
list<sharedClique> childRoots;
|
||||
BOOST_FOREACH(const BayesTree<Conditional>& subtree, subtrees) {
|
||||
nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end());
|
||||
childRoots.push_back(subtree.root());
|
||||
}
|
||||
|
||||
// create a new clique and add all the conditionals to the clique
|
||||
sharedClique new_clique;
|
||||
typename BayesNet<Conditional>::sharedConditional conditional;
|
||||
BOOST_REVERSE_FOREACH(conditional, bayesNet) {
|
||||
if (!new_clique.get()) {
|
||||
new_clique = addClique(conditional,childRoots);
|
||||
} else {
|
||||
nodes_.insert(make_pair(conditional->key(), new_clique));
|
||||
new_clique->push_front(conditional);
|
||||
}
|
||||
}
|
||||
|
||||
root_ = new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
void BayesTree<Conditional>::print(const string& s) const {
|
||||
|
|
@ -358,7 +388,6 @@ namespace gtsam {
|
|||
}
|
||||
cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << endl;
|
||||
if (nodes_.empty()) return;
|
||||
printf("printing tree!\n");
|
||||
root_->printTree("");
|
||||
}
|
||||
|
||||
|
|
@ -430,6 +459,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TODO: remove this function after removing TSAM.cpp
|
||||
template<class Conditional>
|
||||
typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::insert(
|
||||
const BayesNet<Conditional>& bayesNet, list<sharedClique>& children, bool isRootClique)
|
||||
|
|
|
|||
|
|
@ -151,6 +151,12 @@ namespace gtsam {
|
|||
/** Create a Bayes Tree from a Bayes Net */
|
||||
BayesTree(const BayesNet<Conditional>& bayesNet);
|
||||
|
||||
/**
|
||||
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
|
||||
* new root clique and the subtrees are connected to the root clique.
|
||||
*/
|
||||
BayesTree(const BayesNet<Conditional>& bayesNet, std::list<BayesTree<Conditional> > subtrees);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~BayesTree() {
|
||||
}
|
||||
|
|
|
|||
|
|
@ -64,26 +64,46 @@ bool GaussianConditional::equals(const Conditional &c, double tol) const {
|
|||
if (parents_.size() != p->parents_.size()) return false;
|
||||
|
||||
// check if R_ and d_ are equal up to a sign
|
||||
// for (size_t i=0; i<d_.size(); i++) {
|
||||
// Vector row1 = row_(R_, i);
|
||||
// Vector row2 = row_(p->R_, i);
|
||||
// if (!((::equal_with_abs_tol(row1, row2, tol) && fabs(d_(i) - p->d_(i)) < tol) ||
|
||||
// (::equal_with_abs_tol(row1, row2*(-1), tol) && fabs(d_(i) + p->d_(i)) < tol)))
|
||||
// return false;
|
||||
//
|
||||
// float sign = ::equal_with_abs_tol(row1, row2, tol) ? 1 : -1;
|
||||
//
|
||||
// // check if the matrices are the same
|
||||
// // iterate over the parents_ map
|
||||
// for (it = parents_.begin(); it != parents_.end(); it++) {
|
||||
// Parents::const_iterator it2 = p->parents_.find(it->first);
|
||||
// if (it2 != p->parents_.end()) {
|
||||
// if (!::equal_with_abs_tol(row_(it->second*sign, i), row_(it2->second,i), tol))
|
||||
// return false;
|
||||
// } else
|
||||
// return false;
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// check if R_ and d_ are linear independent
|
||||
for (size_t i=0; i<d_.size(); i++) {
|
||||
Vector row1 = row_(R_, i);
|
||||
Vector row2 = row_(p->R_, i);
|
||||
if (!((::equal_with_abs_tol(row1, row2, tol) && fabs(d_(i) - p->d_(i)) < tol) ||
|
||||
(::equal_with_abs_tol(row1, row2*(-1), tol) && fabs(d_(i) + p->d_(i)) < tol)))
|
||||
return false;
|
||||
|
||||
float sign = ::equal_with_abs_tol(row1, row2, tol) ? 1 : -1;
|
||||
list<Vector> rows1; rows1.push_back(row_(R_, i));
|
||||
list<Vector> rows2; rows2.push_back(row_(p->R_, i));
|
||||
|
||||
// check if the matrices are the same
|
||||
// iterate over the parents_ map
|
||||
for (it = parents_.begin(); it != parents_.end(); it++) {
|
||||
Parents::const_iterator it2 = p->parents_.find(it->first);
|
||||
if (it2 != p->parents_.end()) {
|
||||
if (!::equal_with_abs_tol(row_(it->second*sign, i), row_(it2->second,i), tol))
|
||||
return false;
|
||||
rows1.push_back(row_(it->second, i));
|
||||
rows2.push_back(row_(it2->second,i));
|
||||
} else
|
||||
return false;
|
||||
}
|
||||
|
||||
Vector row1 = concatVectors(rows1);
|
||||
Vector row2 = concatVectors(rows2);
|
||||
if (!linear_dependent(row1, row2, tol)) return false;
|
||||
}
|
||||
|
||||
// check if sigmas are equal
|
||||
|
|
|
|||
|
|
@ -379,9 +379,9 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_
|
|||
#include <boost/numeric/ublas/io.hpp>
|
||||
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
||||
|
||||
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
|
||||
pair<GaussianBayesNet, GaussianFactor::shared_ptr>
|
||||
GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
||||
const Ordering& frontal, const Ordering& separator,
|
||||
const Ordering& frontals, const Ordering& separators,
|
||||
const Dimensions& dimensions) {
|
||||
bool verbose = false;
|
||||
|
||||
|
|
@ -389,40 +389,91 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
|||
if (verbose) model->print("Before QR");
|
||||
SharedDiagonal noiseModel = model->QR(Ab);
|
||||
if (verbose) model->print("After QR");
|
||||
// gtsam::print(Ab, "Ab after QR");
|
||||
|
||||
// get dimensions of the eliminated variable
|
||||
// TODO: this is another map find that should be avoided !
|
||||
size_t n1 = dimensions.at(frontal.front()), n = Ab.size2() - 1;
|
||||
size_t n1 = dimensions.at(frontals.front()), n = Ab.size2() - 1;
|
||||
|
||||
// Get alias to augmented RHS d
|
||||
ublas::matrix_column<Matrix> d(Ab,n);
|
||||
|
||||
// // create base conditional Gaussian
|
||||
// GaussianConditional::shared_ptr conditional(new GaussianConditional(frontals.front(),
|
||||
// sub(d, 0, n1), // form d vector
|
||||
// sub(Ab, 0, n1, 0, n1), // form R matrix
|
||||
// sub(noiseModel->sigmas(),0,n1))); // get standard deviations
|
||||
//
|
||||
// // extract the block matrices for parents in both CG and LF
|
||||
// GaussianFactor::shared_ptr factor(new GaussianFactor);
|
||||
// size_t j = n1;
|
||||
// BOOST_FOREACH(const Symbol& cur_key, separators) {
|
||||
// size_t dim = dimensions.at(cur_key); // TODO avoid find !
|
||||
// conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim));
|
||||
// factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
|
||||
// j+=dim;
|
||||
// }
|
||||
//
|
||||
// // Set sigmas
|
||||
// // set the right model here
|
||||
// if (noiseModel->isConstrained())
|
||||
// factor->model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(),n1,maxRank));
|
||||
// else
|
||||
// factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank));
|
||||
//
|
||||
// // extract ds vector for the new b
|
||||
// factor->set_b(sub(d, n1, maxRank));
|
||||
//
|
||||
// return make_pair(conditional, factor);
|
||||
|
||||
// extract the conditionals
|
||||
GaussianBayesNet bn;
|
||||
size_t n0 = 0;
|
||||
Ordering::const_iterator itFrontal1 = frontals.begin(), itFrontal2;
|
||||
for(; itFrontal1!=frontals.end(); itFrontal1++) {
|
||||
n1 = n0 + dimensions.at(*itFrontal1);
|
||||
// create base conditional Gaussian
|
||||
GaussianConditional::shared_ptr conditional(new GaussianConditional(*itFrontal1,
|
||||
sub(d, n0, n1), // form d vector
|
||||
sub(Ab, n0, n1, n0, n1), // form R matrix
|
||||
sub(noiseModel->sigmas(),n0,n1))); // get standard deviations
|
||||
|
||||
// add parents to the conditional
|
||||
itFrontal2 = itFrontal1;
|
||||
itFrontal2 ++;
|
||||
size_t j = n1;
|
||||
for (; itFrontal2!=frontals.end(); itFrontal2++) {
|
||||
size_t dim = dimensions.at(*itFrontal2);
|
||||
conditional->add(*itFrontal2, sub(Ab, n0, n1, j, j+dim));
|
||||
j+=dim;
|
||||
}
|
||||
BOOST_FOREACH(const Symbol& cur_key, separators) {
|
||||
size_t dim = dimensions.at(cur_key);
|
||||
conditional->add(cur_key, sub(Ab, n0, n1, j, j+dim));
|
||||
j+=dim;
|
||||
}
|
||||
n0 = n1;
|
||||
bn.push_back(conditional);
|
||||
}
|
||||
|
||||
// if m<n1, this factor cannot be eliminated
|
||||
size_t maxRank = noiseModel->dim();
|
||||
if (maxRank<n1) {
|
||||
cout << "Perhaps your factor graph is singular." << endl;
|
||||
cout << "Here are the keys involved in the factor now being eliminated:" << endl;
|
||||
separator.print("Keys");
|
||||
cout << "The first key, '" << (string)frontal.front() << "', corresponds to the variable being eliminated" << endl;
|
||||
separators.print("Keys");
|
||||
cout << "The first key, '" << (string)frontals.front() << "', corresponds to the variable being eliminated" << endl;
|
||||
throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
|
||||
}
|
||||
|
||||
// Get alias to augmented RHS d
|
||||
ublas::matrix_column<Matrix> d(Ab,n);
|
||||
|
||||
// create base conditional Gaussian
|
||||
GaussianConditional::shared_ptr conditional(new GaussianConditional(frontal.front(),
|
||||
sub(d, 0, n1), // form d vector
|
||||
sub(Ab, 0, n1, 0, n1), // form R matrix
|
||||
sub(noiseModel->sigmas(),0,n1))); // get standard deviations
|
||||
|
||||
// extract the block matrices for parents in both CG and LF
|
||||
// extract the new factor
|
||||
GaussianFactor::shared_ptr factor(new GaussianFactor);
|
||||
size_t j = n1;
|
||||
BOOST_FOREACH(const Symbol& cur_key, separator)
|
||||
if (cur_key!=frontal.front()) {
|
||||
size_t dim = dimensions.at(cur_key); // TODO avoid find !
|
||||
conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim));
|
||||
factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
|
||||
j+=dim;
|
||||
}
|
||||
BOOST_FOREACH(const Symbol& cur_key, separators) {
|
||||
size_t dim = dimensions.at(cur_key); // TODO avoid find !
|
||||
factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
|
||||
j+=dim;
|
||||
}
|
||||
|
||||
// Set sigmas
|
||||
// set the right model here
|
||||
|
|
@ -434,9 +485,20 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
|||
// extract ds vector for the new b
|
||||
factor->set_b(sub(d, n1, maxRank));
|
||||
|
||||
return make_pair(conditional, factor);
|
||||
return make_pair(bn, factor);
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
|
||||
GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
||||
const Symbol& frontal, const Ordering& separator,
|
||||
const Dimensions& dimensions) {
|
||||
Ordering frontals; frontals += frontal;
|
||||
pair<GaussianBayesNet, shared_ptr> ret =
|
||||
eliminateMatrix(Ab, model, frontals, separator, dimensions);
|
||||
return make_pair(*ret.first.begin(), ret.second);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
|
||||
GaussianFactor::eliminate(const Symbol& key) const
|
||||
|
|
@ -451,8 +513,7 @@ GaussianFactor::eliminate(const Symbol& key) const
|
|||
}
|
||||
|
||||
// create an internal ordering that eliminates key first
|
||||
Ordering frontal, ordering;
|
||||
frontal += key;
|
||||
Ordering ordering;
|
||||
ordering += key;
|
||||
BOOST_FOREACH(const Symbol& k, keys())
|
||||
if (k != key) ordering += k;
|
||||
|
|
@ -462,7 +523,7 @@ GaussianFactor::eliminate(const Symbol& key) const
|
|||
|
||||
// TODO: this is where to split
|
||||
ordering.pop_front();
|
||||
return eliminateMatrix(Ab, model_, frontal, ordering, dimensions());
|
||||
return eliminateMatrix(Ab, model_, key, ordering, dimensions());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include "VectorConfig.h"
|
||||
#include "SharedDiagonal.h"
|
||||
#include "GaussianConditional.h" // Needed for MATLAB
|
||||
#include "GaussianBayesNet.h"
|
||||
#include "SymbolMap.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -250,11 +251,17 @@ public:
|
|||
* Performs elimination given an augmented matrix
|
||||
* @param
|
||||
*/
|
||||
static std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
|
||||
static std::pair<GaussianBayesNet, shared_ptr>
|
||||
eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
||||
const Ordering& frontal, const Ordering& separator,
|
||||
const Dimensions& dimensions);
|
||||
|
||||
static std::pair<GaussianConditional::shared_ptr, shared_ptr>
|
||||
eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
||||
const Symbol& frontal, const Ordering& separator,
|
||||
const Dimensions& dimensions);
|
||||
|
||||
|
||||
/**
|
||||
* Take the factor f, and append to current matrices. Not very general.
|
||||
* @param f linear factor graph
|
||||
|
|
|
|||
|
|
@ -216,7 +216,7 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
|
|||
}
|
||||
|
||||
// add the keys to the rendering
|
||||
Ordering frontal, render; frontal += key; render += key;
|
||||
Ordering render; render += key;
|
||||
BOOST_FOREACH(const Symbol& k, separator)
|
||||
if (k != key) render += k;
|
||||
|
||||
|
|
@ -230,7 +230,7 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
|
|||
GaussianConditional::shared_ptr conditional;
|
||||
render.pop_front();
|
||||
boost::tie(conditional, factor) =
|
||||
GaussianFactor::eliminateMatrix(Ab, model, frontal, render, dimensions);
|
||||
GaussianFactor::eliminateMatrix(Ab, model, key, render, dimensions);
|
||||
|
||||
// add new factor on separator back into the graph
|
||||
if (!factor->empty()) push_back(factor);
|
||||
|
|
@ -255,16 +255,31 @@ GaussianFactorGraph::eliminate(const Ordering& ordering, bool old)
|
|||
GaussianBayesNet
|
||||
GaussianFactorGraph::eliminateFrontals(const Ordering& frontals)
|
||||
{
|
||||
Matrix Ab; SharedDiagonal model;
|
||||
// find the factors that contain at least one of the frontal variables
|
||||
Dimensions dimensions = this->dimensions();
|
||||
boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, keys(), dimensions);
|
||||
|
||||
// collect separator
|
||||
Ordering separator;
|
||||
set<Symbol> frontal_set(frontals.begin(), frontals.end());
|
||||
BOOST_FOREACH(const Symbol& key, this->keys()) {
|
||||
if (frontal_set.find(key) == frontal_set.end())
|
||||
separator.push_back(key);
|
||||
}
|
||||
|
||||
Matrix Ab; SharedDiagonal model;
|
||||
Ordering ord = frontals;
|
||||
ord.insert(ord.end(), separator.begin(), separator.end());
|
||||
boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, ord, dimensions);
|
||||
|
||||
// eliminate that joint factor
|
||||
GaussianFactor::shared_ptr factor;
|
||||
// GaussianConditional::shared_ptr conditional;
|
||||
GaussianBayesNet bn;
|
||||
// boost::tie(bn, factor) =
|
||||
// GaussianFactor::eliminateMatrix(Ab, model, frontals, dimensions);
|
||||
boost::tie(bn, factor) =
|
||||
GaussianFactor::eliminateMatrix(Ab, model, frontals, separator, dimensions);
|
||||
|
||||
// add new factor on separator back into the graph
|
||||
*this = GaussianFactorGraph();
|
||||
if (!factor->empty()) push_back(factor);
|
||||
|
||||
return bn;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ namespace gtsam {
|
|||
template <class FG>
|
||||
void JunctionTree<FG>::Clique::print(const string& indent) const {
|
||||
// FG::print(indent);
|
||||
cout << "kai1" << endl;
|
||||
cout << indent;
|
||||
BOOST_FOREACH(const Symbol& key, frontal_)
|
||||
cout << (string)key << " ";
|
||||
|
|
@ -70,7 +69,6 @@ namespace gtsam {
|
|||
SymbolicFactorGraph sfg(fg);
|
||||
SymbolicBayesNet sbn = sfg.eliminate(ordering);
|
||||
BayesTree<SymbolicConditional> sbt(sbn);
|
||||
sbt.print("sbt");
|
||||
|
||||
// distribtue factors
|
||||
root_ = distributeFactors(fg, sbt.root());
|
||||
|
|
@ -106,39 +104,49 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template <class FG> template <class Conditional>
|
||||
pair<FG, typename BayesTree<Conditional>::sharedClique>
|
||||
JunctionTree<FG>::eliminateOneClique(sharedClique current, BayesTree<Conditional>& bayesTree) {
|
||||
pair<FG, BayesTree<Conditional> >
|
||||
JunctionTree<FG>::eliminateOneClique(sharedClique current) {
|
||||
|
||||
// current->frontal_.print("current clique:");
|
||||
|
||||
typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
|
||||
FG fg; // factor graph will be assembled from local factors and marginalized children
|
||||
list<sharedBtreeClique> children;
|
||||
list<BayesTree<Conditional> > children;
|
||||
fg.push_back(*current); // add the local factor graph
|
||||
|
||||
// BOOST_FOREACH(const typename FG::sharedFactor& factor_, fg)
|
||||
// Ordering(factor_->keys()).print("local factor:");
|
||||
|
||||
BOOST_FOREACH(sharedClique& child, current->children_) {
|
||||
// receive the factors from the child and its clique point
|
||||
FG fgChild; sharedBtreeClique cliqueChild;
|
||||
boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree);
|
||||
if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!");
|
||||
FG fgChild; BayesTree<Conditional> childTree;
|
||||
boost::tie(fgChild, childTree) = eliminateOneClique<Conditional>(child);
|
||||
|
||||
// BOOST_FOREACH(const typename FG::sharedFactor& factor_, fgChild)
|
||||
// Ordering(factor_->keys()).print("factor from child:");
|
||||
|
||||
fg.push_back(fgChild);
|
||||
children.push_back(cliqueChild);
|
||||
children.push_back(childTree);
|
||||
}
|
||||
|
||||
// eliminate the combined factors
|
||||
// warning: fg is being eliminated in-place and will contain marginal afterwards
|
||||
// BayesNet<Conditional> bn = fg.eliminate(current->frontal_);
|
||||
BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
|
||||
|
||||
// create a new clique corresponding the combined factors
|
||||
sharedBtreeClique new_clique = bayesTree.insert(bn, children);
|
||||
BayesTree<Conditional> bayesTree(bn, children);
|
||||
|
||||
return make_pair(fg, new_clique);
|
||||
return make_pair(fg, bayesTree);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG> template <class Conditional>
|
||||
BayesTree<Conditional> JunctionTree<FG>::eliminate() {
|
||||
BayesTree<Conditional> bayesTree;
|
||||
eliminateOneClique(root_, bayesTree);
|
||||
return bayesTree;
|
||||
pair<FG, BayesTree<Conditional> > ret = this->eliminateOneClique<Conditional>(root_);
|
||||
// ret.first.print("ret.first");
|
||||
if (ret.first.nrFactors() != 0)
|
||||
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
|
||||
return ret.second;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -173,7 +181,8 @@ namespace gtsam {
|
|||
VectorConfig GaussianJunctionTree<FG>::optimize() {
|
||||
// eliminate from leaves to the root
|
||||
typedef JunctionTree<FG> Base;
|
||||
BayesTree<GaussianConditional> bayesTree = this->eliminate<GaussianConditional>();
|
||||
BayesTree<GaussianConditional> bayesTree;
|
||||
this->eliminate<GaussianConditional>();
|
||||
|
||||
// back-substitution
|
||||
VectorConfig result;
|
||||
|
|
|
|||
|
|
@ -73,10 +73,9 @@ namespace gtsam {
|
|||
// distribute the factors along the Bayes tree
|
||||
sharedClique distributeFactors(FG& fg, const BayesTree<SymbolicConditional>::sharedClique clique);
|
||||
|
||||
// utility function called by eliminateBottomUp
|
||||
// utility function called by eliminate
|
||||
template <class Conditional>
|
||||
std::pair<FG, typename BayesTree<Conditional>::sharedClique> eliminateOneClique(
|
||||
sharedClique fg_, BayesTree<Conditional>& bayesTree);
|
||||
std::pair<FG, BayesTree<Conditional> > eliminateOneClique(sharedClique fg_);
|
||||
|
||||
public:
|
||||
// constructor
|
||||
|
|
@ -126,6 +125,6 @@ namespace gtsam {
|
|||
|
||||
// optimize the linear graph
|
||||
VectorConfig optimize();
|
||||
}; // Linear JunctionTree
|
||||
}; // GaussianJunctionTree
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -809,6 +809,38 @@ TEST( GaussianFactor, erase)
|
|||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactor, eliminateMatrix)
|
||||
{
|
||||
Matrix Ab = Matrix_(3, 4,
|
||||
1., 2., 0., 3.,
|
||||
0., 4., 5., 6.,
|
||||
0., 0., 7., 8.);
|
||||
SharedDiagonal model(Vector_(3, 0.5, 0.5, 0.5));
|
||||
Ordering frontals; frontals += "x1", "x2";
|
||||
Ordering separator; separator += "x3";
|
||||
Dimensions dimensions;
|
||||
dimensions.insert(make_pair("x1", 1));
|
||||
dimensions.insert(make_pair("x2", 1));
|
||||
dimensions.insert(make_pair("x3", 1));
|
||||
|
||||
GaussianFactor::shared_ptr factor;
|
||||
GaussianBayesNet bn;
|
||||
boost::tie(bn, factor) =
|
||||
GaussianFactor::eliminateMatrix(Ab, model, frontals, separator, dimensions);
|
||||
|
||||
GaussianBayesNet bn_expected;
|
||||
GaussianBayesNet::sharedConditional conditional1(new GaussianConditional("x1", Vector_(1, 6.), Matrix_(1, 1, 2.),
|
||||
"x2", Matrix_(1, 1, 4.), "x3", Matrix_(1, 1, 0.), Vector_(1, 1.)));
|
||||
GaussianBayesNet::sharedConditional conditional2(new GaussianConditional("x2", Vector_(1, 12.), Matrix_(1, 1, 8.),
|
||||
"x3", Matrix_(1, 1, 10.), Vector_(1, 1.)));
|
||||
bn_expected.push_back(conditional1);
|
||||
bn_expected.push_back(conditional2);
|
||||
CHECK(assert_equal(bn_expected, bn));
|
||||
|
||||
GaussianFactor factor_expected("x3", Matrix_(1, 1, 14.), Vector_(1, 16.), SharedDiagonal(Vector_(1, 1.)));
|
||||
CHECK(assert_equal(factor_expected, *factor));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
|
|
|
|||
|
|
@ -934,6 +934,42 @@ TEST ( GaussianFactorGraph, combine_matrix ) {
|
|||
CHECK(assert_equal(*expModel, *noise));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* x2 x1 x3 b
|
||||
* 1 1 1 1 1 0 1
|
||||
* 1 1 1 -> 1 1 1
|
||||
* 1 1 1 1
|
||||
*/
|
||||
TEST ( GaussianFactorGraph, eliminateFrontals ) {
|
||||
typedef GaussianFactorGraph::sharedFactor Factor;
|
||||
SharedDiagonal model(Vector_(1, 0.5));
|
||||
GaussianFactorGraph fg;
|
||||
Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
fg.push_back(factor1);
|
||||
fg.push_back(factor2);
|
||||
fg.push_back(factor3);
|
||||
|
||||
Ordering frontals; frontals += "x2", "x1";
|
||||
GaussianBayesNet bn = fg.eliminateFrontals(frontals);
|
||||
|
||||
GaussianBayesNet bn_expected;
|
||||
GaussianBayesNet::sharedConditional conditional1(new GaussianConditional("x2", Vector_(1, 2.), Matrix_(1, 1, 2.),
|
||||
"x1", Matrix_(1, 1, 1.), "x3", Matrix_(1, 1, 1.), Vector_(1, 1.)));
|
||||
GaussianBayesNet::sharedConditional conditional2(new GaussianConditional("x1", Vector_(1, 0.), Matrix_(1, 1, -1.),
|
||||
"x3", Matrix_(1, 1, 1.), Vector_(1, 1.)));
|
||||
bn_expected.push_back(conditional1);
|
||||
bn_expected.push_back(conditional2);
|
||||
CHECK(assert_equal(bn_expected, bn));
|
||||
|
||||
GaussianFactorGraph::sharedFactor factor_expected(new GaussianFactor("x3", Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
|
||||
GaussianFactorGraph fg_expected;
|
||||
fg_expected.push_back(factor_expected);
|
||||
CHECK(assert_equal(fg_expected, fg));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -23,6 +23,40 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using namespace example;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* x1 - x2 - x3 - x4
|
||||
* x3 x4
|
||||
* x2 x1 : x3
|
||||
*/
|
||||
TEST( GaussianFactorGraph, constructor )
|
||||
{
|
||||
typedef GaussianFactorGraph::sharedFactor Factor;
|
||||
SharedDiagonal model(Vector_(1, 0.2));
|
||||
Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
|
||||
GaussianFactorGraph fg;
|
||||
fg.push_back(factor1);
|
||||
fg.push_back(factor2);
|
||||
fg.push_back(factor3);
|
||||
|
||||
Ordering ordering; ordering += "x2","x1","x3","x4";
|
||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
||||
|
||||
Ordering frontal1; frontal1 += "x3", "x4";
|
||||
Ordering frontal2; frontal2 += "x2", "x1";
|
||||
Unordered sep1;
|
||||
Unordered sep2; sep2 += "x3";
|
||||
CHECK(assert_equal(frontal1, junctionTree.root()->frontal()));
|
||||
CHECK(assert_equal(sep1, junctionTree.root()->separator()));
|
||||
LONGS_EQUAL(1, junctionTree.root()->size());
|
||||
CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal()));
|
||||
CHECK(assert_equal(sep2, junctionTree.root()->children()[0]->separator()));
|
||||
LONGS_EQUAL(2, junctionTree.root()->children()[0]->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
Bayes tree for smoother with "nested dissection" ordering:
|
||||
C1 x5 x6 x4
|
||||
|
|
@ -30,7 +64,7 @@ using namespace example;
|
|||
C3 x1 : x2
|
||||
C4 x7 : x6
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, constructor )
|
||||
TEST( GaussianFactorGraph, constructor2 )
|
||||
{
|
||||
// create a graph
|
||||
GaussianFactorGraph fg = createSmoother(7);
|
||||
|
|
@ -61,8 +95,51 @@ TEST( GaussianFactorGraph, constructor )
|
|||
LONGS_EQUAL(2, junctionTree.root()->children()[1]->size());
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* x1 - x2 - x3 - x4
|
||||
* x3 x4
|
||||
* x2 x1 : x3
|
||||
*
|
||||
* x2 x1 x3 x4 b
|
||||
* 1 1 1
|
||||
* 1 1 1
|
||||
* 1 1 1
|
||||
* 1 1
|
||||
*
|
||||
* 1 0 0 1
|
||||
*/
|
||||
TEST( GaussianFactorGraph, eliminate )
|
||||
{
|
||||
typedef GaussianFactorGraph::sharedFactor Factor;
|
||||
SharedDiagonal model(Vector_(1, 0.5));
|
||||
Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
Factor factor4(new GaussianFactor("x4", Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
|
||||
GaussianFactorGraph fg;
|
||||
fg.push_back(factor1);
|
||||
fg.push_back(factor2);
|
||||
fg.push_back(factor3);
|
||||
fg.push_back(factor4);
|
||||
|
||||
Ordering ordering; ordering += "x2","x1","x3","x4";
|
||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
||||
BayesTree<GaussianConditional> bayesTree = junctionTree.eliminate<GaussianConditional>();
|
||||
|
||||
typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
|
||||
Matrix two = Matrix_(1,1,2.);
|
||||
Matrix one = Matrix_(1,1,1.);
|
||||
BayesTree<GaussianConditional> bayesTree_expected;
|
||||
bayesTree_expected.insert(sharedConditional(new GaussianConditional("x4", Vector_(1,2.), two, Vector_(1,1.))), ordering);
|
||||
bayesTree_expected.insert(sharedConditional(new GaussianConditional("x3", Vector_(1,2.), two, "x4", two, Vector_(1,1.))), ordering);
|
||||
bayesTree_expected.insert(sharedConditional(new GaussianConditional("x1", Vector_(1,0.), one*(-1), "x3", one, Vector_(1,1.))), ordering);
|
||||
bayesTree_expected.insert(sharedConditional(new GaussianConditional("x2", Vector_(1,2.), two, "x1", one, "x3", one, Vector_(1,1.))), ordering);
|
||||
CHECK(assert_equal(bayesTree_expected, bayesTree));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
TEST( GaussianFactorGraph, optimizeMultiFrontal )
|
||||
{
|
||||
// create a graph
|
||||
|
|
@ -72,10 +149,10 @@ TEST( GaussianFactorGraph, optimizeMultiFrontal )
|
|||
Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||
|
||||
// optimize the graph
|
||||
// GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
||||
// VectorConfig actual = junctionTree.optimize();
|
||||
//
|
||||
// // verify
|
||||
GaussianJunctionTree<GaussianFactorGraph> junctionTree(fg, ordering);
|
||||
VectorConfig actual = junctionTree.optimize();
|
||||
|
||||
// verify
|
||||
// VectorConfig expected = createCorrectDelta();
|
||||
//
|
||||
// CHECK(assert_equal(expected,actual));
|
||||
|
|
|
|||
Loading…
Reference in New Issue