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

(in branch) Dogleg in ISAM2 in progress

release/4.3a0
Richard Roberts 2011-11-12 21:19:46 +00:00
parent e78668bad9
commit e6a43d6330
16 changed files with 383 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
gtsam/inference/BayesTree-inl.h
View File

@ -44,7 +44,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::assertInvariants() const { void BayesTreeClique<CONDITIONAL>::assertInvariants() const {
#ifndef NDEBUG #ifndef NDEBUG
// We rely on the keys being sorted // We rely on the keys being sorted
// FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end()); // FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
@ -57,19 +57,19 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) : conditional_(conditional) { BayesTreeClique<CONDITIONAL>::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) {
assertInvariants(); assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::print(const string& s) const { void BayesTreeClique<CONDITIONAL>::print(const string& s) const {
conditional_->print(s); conditional_->print(s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
size_t BayesTree<CONDITIONAL>::Clique::treeSize() const { size_t BayesTreeClique<CONDITIONAL>::treeSize() const {
size_t size = 1; size_t size = 1;
BOOST_FOREACH(const shared_ptr& child, children_) BOOST_FOREACH(const shared_ptr& child, children_)
size += child->treeSize(); size += child->treeSize();
@ -78,7 +78,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::printTree(const string& indent) const { void BayesTreeClique<CONDITIONAL>::printTree(const string& indent) const {
print(indent); print(indent);
BOOST_FOREACH(const shared_ptr& child, children_) BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent+" "); child->printTree(indent+" ");
@ -86,9 +86,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::permuteWithInverse(const Permutation& inversePermutation) { void BayesTreeClique<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation); conditional_->permuteWithInverse(inversePermutation);
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) { BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation); child->permuteWithInverse(inversePermutation);
} }
@ -97,7 +96,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
bool BayesTree<CONDITIONAL>::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { bool BayesTreeClique<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation);
#ifndef NDEBUG #ifndef NDEBUG
if(!changed) { if(!changed) {
@ -108,7 +107,6 @@ namespace gtsam {
} }
#endif #endif
if(changed) { if(changed) {
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) { BOOST_FOREACH(const shared_ptr& child, children_) {
(void)child->permuteSeparatorWithInverse(inversePermutation); (void)child->permuteSeparatorWithInverse(inversePermutation);
} }
@ -118,17 +116,17 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL>::CliqueData typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
BayesTree<CONDITIONAL>::getCliqueData() const { BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const {
CliqueData data; CliqueData data;
getCliqueData(data, root_); getCliqueData(data, root_);
return data; return data;
} }
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::getCliqueData(CliqueData& data, void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data,
BayesTree<CONDITIONAL>::sharedClique clique) const { BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique) const {
data.conditionalSizes.push_back((*clique)->nrFrontals()); data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents()); data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) { BOOST_FOREACH(sharedClique c, clique->children_) {
@ -137,8 +135,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::saveGraph(const std::string &s) const { void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s) const {
if (!root_.get()) throw invalid_argument("the root of bayes tree has not been initialized!"); if (!root_.get()) throw invalid_argument("the root of bayes tree has not been initialized!");
ofstream of(s.c_str()); ofstream of(s.c_str());
of<< "digraph G{\n"; of<< "digraph G{\n";
@ -147,9 +145,9 @@ namespace gtsam {
of.close(); of.close();
} }
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::saveGraph(ostream &s, void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s,
BayesTree<CONDITIONAL>::sharedClique clique, BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique,
int parentnum) const { int parentnum) const {
static int num = 0; static int num = 0;
bool first = true; bool first = true;
@ -184,9 +182,9 @@ namespace gtsam {
} }
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL>::CliqueStats typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
BayesTree<CONDITIONAL>::CliqueData::getStats() const { BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
CliqueStats stats; CliqueStats stats;
double sum = 0.0; double sum = 0.0;
@ -216,7 +214,7 @@ namespace gtsam {
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R, BayesNet<CONDITIONAL> BayesTreeClique<CONDITIONAL>::shortcut(shared_ptr R,
Eliminate function) { Eliminate function) {
static const bool debug = false; static const bool debug = false;
@ -224,7 +222,7 @@ namespace gtsam {
// A first base case is when this clique or its parent is the root, // A first base case is when this clique or its parent is the root,
// in which case we return an empty Bayes net. // in which case we return an empty Bayes net.
sharedClique parent(parent_.lock()); shared_ptr parent(parent_.lock());
if (R.get()==this || parent==R) { if (R.get()==this || parent==R) {
BayesNet<CONDITIONAL> empty; BayesNet<CONDITIONAL> empty;
@ -318,7 +316,7 @@ namespace gtsam {
// Because the root clique could be very big. // Because the root clique could be very big.
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::marginal( FactorGraph<typename CONDITIONAL::FactorType> BayesTreeClique<CONDITIONAL>::marginal(
shared_ptr R, Eliminate function) { shared_ptr R, Eliminate function) {
// If we are the root, just return this root // If we are the root, just return this root
// NOTE: immediately cast to a factor graph // NOTE: immediately cast to a factor graph
@ -339,7 +337,7 @@ namespace gtsam {
// P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::joint( FactorGraph<typename CONDITIONAL::FactorType> BayesTreeClique<CONDITIONAL>::joint(
shared_ptr C2, shared_ptr R, Eliminate function) { shared_ptr C2, shared_ptr R, Eliminate function) {
// For now, assume neither is the root // For now, assume neither is the root
@ -367,23 +365,23 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::Cliques::print(const std::string& s) const { void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s) const {
cout << s << ":\n"; cout << s << ":\n";
BOOST_FOREACH(sharedClique clique, *this) BOOST_FOREACH(sharedClique clique, *this)
clique->printTree(); clique->printTree();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
bool BayesTree<CONDITIONAL>::Cliques::equals(const Cliques& other, double tol) const { bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
return other == *this; return other == *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique( typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique
const sharedConditional& conditional, sharedClique parent_clique) { BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, sharedClique parent_clique) {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, conditional->frontals()) BOOST_FOREACH(Index key, conditional->frontals())
@ -397,8 +395,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique( typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
const sharedConditional& conditional, list<sharedClique>& child_cliques) { const sharedConditional& conditional, list<sharedClique>& child_cliques) {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
@ -412,8 +410,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
inline void BayesTree<CONDITIONAL>::addToCliqueFront(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false; static const bool debug = false;
#ifndef NDEBUG #ifndef NDEBUG
// Debug check to make sure the conditional variable is ordered lower than // Debug check to make sure the conditional variable is ordered lower than
@ -439,8 +437,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::removeClique(sharedClique clique) { void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
if (clique->isRoot()) if (clique->isRoot())
root_.reset(); root_.reset();
@ -449,7 +447,7 @@ namespace gtsam {
// orphan my children // orphan my children
BOOST_FOREACH(sharedClique child, clique->children_) BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename BayesTree<CONDITIONAL>::Clique::weak_ptr(); child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Index key, (*clique->conditional())) { BOOST_FOREACH(Index key, (*clique->conditional())) {
nodes_[key].reset(); nodes_[key].reset();
@ -457,27 +455,28 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL>::BayesTree() { BayesTree<CONDITIONAL,CLIQUE>::BayesTree() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) { BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) {
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit; typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*this, *rit); insert(*this, *rit);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees) { BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees) {
if (bayesNet.size() == 0) if (bayesNet.size() == 0)
throw invalid_argument("BayesTree::insert: empty bayes net!"); throw invalid_argument("BayesTree::insert: empty bayes net!");
// get the roots of child subtrees and merge their nodes_ // get the roots of child subtrees and merge their nodes_
list<sharedClique> childRoots; list<sharedClique> childRoots;
BOOST_FOREACH(const BayesTree<CONDITIONAL>& subtree, subtrees) { typedef BayesTree<CONDITIONAL,CLIQUE> Tree;
BOOST_FOREACH(const Tree& subtree, subtrees) {
nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end()); nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end());
childRoots.push_back(subtree.root()); childRoots.push_back(subtree.root());
} }
@ -496,8 +495,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::print(const string& s) const { void BayesTree<CONDITIONAL,CLIQUE>::print(const string& s) const {
if (root_.use_count() == 0) { if (root_.use_count() == 0) {
printf("WARNING: BayesTree.print encountered a forest...\n"); printf("WARNING: BayesTree.print encountered a forest...\n");
return; return;
@ -509,26 +508,26 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// binary predicate to test equality of a pair for use in equals // binary predicate to test equality of a pair for use in equals
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
bool check_sharedCliques( bool check_sharedCliques(
const typename BayesTree<CONDITIONAL>::sharedClique& v1, const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1,
const typename BayesTree<CONDITIONAL>::sharedClique& v2 const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2
) { ) {
return v1->equals(*v2); return v1->equals(*v2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
bool BayesTree<CONDITIONAL>::equals(const BayesTree<CONDITIONAL>& other, bool BayesTree<CONDITIONAL,CLIQUE>::equals(const BayesTree<CONDITIONAL,CLIQUE>& other,
double tol) const { double tol) const {
return size()==other.size() && return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL>); std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER> template<class CONTAINER>
inline Index BayesTree<CONDITIONAL>::findParentClique(const CONTAINER& parents) const { inline Index BayesTree<CONDITIONAL,CLIQUE>::findParentClique(const CONTAINER& parents) const {
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end()); assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent; return *lowestOrderedParent;
@ -548,8 +547,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::insert(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional) void BayesTree<CONDITIONAL,CLIQUE>::insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional)
{ {
static const bool debug = false; static const bool debug = false;
@ -597,8 +596,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
//TODO: remove this function after removing TSAM.cpp //TODO: remove this function after removing TSAM.cpp
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::insert( typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert(
const sharedConditional& clique, list<sharedClique>& children, bool isRootClique) { const sharedConditional& clique, list<sharedClique>& children, bool isRootClique) {
if (clique->nrFrontals() == 0) if (clique->nrFrontals() == 0)
@ -612,18 +611,19 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::fillNodesIndex(const sharedClique& subtree) { void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node // Add each frontal variable of this root node
BOOST_FOREACH(const Index& key, subtree->conditional()->frontals()) { nodes_[key] = subtree; } BOOST_FOREACH(const Index& key, subtree->conditional()->frontals()) { nodes_[key] = subtree; }
// Fill index for each child // Fill index for each child
BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, subtree->children_) { typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children_) {
fillNodesIndex(child); } fillNodesIndex(child); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::insert(const sharedClique& subtree) { void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
if(subtree) { if(subtree) {
// Find the parent clique of the new subtree. By the running intersection // Find the parent clique of the new subtree. By the running intersection
// property, those separator variables in the subtree that are ordered // property, those separator variables in the subtree that are ordered
@ -651,8 +651,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// First finds clique marginal then marginalizes that // First finds clique marginal then marginalizes that
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL>::marginalFactor( typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor(
Index key, Eliminate function) const { Index key, Eliminate function) const {
// get clique containing key // get clique containing key
@ -666,8 +666,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet( typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet(
Index key, Eliminate function) const { Index key, Eliminate function) const {
// calculate marginal as a factor graph // calculate marginal as a factor graph
@ -681,9 +681,9 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Find two cliques, their joint, then marginalizes // Find two cliques, their joint, then marginalizes
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr BayesTree< typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
CONDITIONAL>::joint(Index key1, Index key2, Eliminate function) const { BayesTree<CONDITIONAL,CLIQUE>::joint(Index key1, Index key2, Eliminate function) const {
// get clique C1 and C2 // get clique C1 and C2
sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
@ -698,8 +698,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet( typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet(
Index key1, Index key2, Eliminate function) const { Index key1, Index key2, Eliminate function) const {
// eliminate factor graph marginal to a Bayes net // eliminate factor graph marginal to a Bayes net
@ -708,17 +708,17 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::clear() { void BayesTree<CONDITIONAL,CLIQUE>::clear() {
// Remove all nodes and clear the root pointer // Remove all nodes and clear the root pointer
nodes_.clear(); nodes_.clear();
root_.reset(); root_.reset();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL>::removePath(sharedClique clique, void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) { BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// base case is NULL, if so we do nothing and return empties above // base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) { if (clique!=NULL) {
@ -730,7 +730,7 @@ namespace gtsam {
this->removeClique(clique); this->removeClique(clique);
// remove path above me // remove path above me
this->removePath(clique->parent_.lock(), bn, orphans); this->removePath(sharedClique(clique->parent_.lock()), bn, orphans);
// add children to list of orphans (splice also removed them from clique->children_) // add children to list of orphans (splice also removed them from clique->children_)
orphans.splice (orphans.begin(), clique->children_); orphans.splice (orphans.begin(), clique->children_);
@ -741,10 +741,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER> template<class CONTAINER>
void BayesTree<CONDITIONAL>::removeTop(const CONTAINER& keys, void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) { BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(const Index& key, keys) { BOOST_FOREACH(const Index& key, keys) {

243
gtsam/inference/BayesTree.h
View File

@ -33,14 +33,19 @@
namespace gtsam { namespace gtsam {
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
template<class CONDITIONAL> class BayesTreeClique;
/** /**
* Bayes tree * Bayes tree
* Templated on the CONDITIONAL class, the type of node in the underlying Bayes chain. * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
* This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional * which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
* *
* \ingroup Multifrontal * \ingroup Multifrontal
*/ */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTree { class BayesTree {
public: public:
@ -52,100 +57,7 @@ namespace gtsam {
typedef typename CONDITIONAL::FactorType FactorType; typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate; typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
/** typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*/
struct Clique {
protected:
void assertInvariants() const;
public:
typedef CONDITIONAL ConditionalType;
typedef typename boost::shared_ptr<Clique> shared_ptr;
typedef typename boost::weak_ptr<Clique> weak_ptr;
sharedConditional conditional_;
weak_ptr parent_;
std::list<shared_ptr> children_;
typename FactorType::shared_ptr cachedFactor_;
friend class BayesTree<CONDITIONAL>;
//* Constructor */
Clique() {}
Clique(const sharedConditional& conditional);
/** print this node */
void print(const std::string& s = "") const;
/** The arrow operator accesses the conditional */
const CONDITIONAL* operator->() const { return conditional_.get(); }
/** The arrow operator accesses the conditional */
CONDITIONAL* operator->() { return conditional_.get(); }
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired(); }
/** return the const reference of children */
std::list<shared_ptr>& children() { return children_; }
const std::list<shared_ptr>& children() const { return children_; }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** Access the cached factor (this is a hack) */
typename FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** print this node and entire subtree below it */
void printTree(const std::string& indent="") const;
/** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation);
/** Permute variables when they only appear in the separators. In this
* case the running intersection property will be used to prevent always
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version
BayesNet<CONDITIONAL> shortcut(shared_ptr root, Eliminate function);
/** return the marginal P(C) of the clique */
FactorGraph<FactorType> marginal(shared_ptr root, Eliminate function);
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<FactorType> joint(shared_ptr C2, shared_ptr root, Eliminate function);
bool equals(const Clique& other, double tol=1e-9) const {
return (!conditional_ && !other.conditional()) ||
conditional_->equals(*(other.conditional()), tol);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
}
}; // \struct Clique
// typedef for shared pointers to cliques // typedef for shared pointers to cliques
typedef boost::shared_ptr<Clique> sharedClique; typedef boost::shared_ptr<Clique> sharedClique;
@ -207,7 +119,7 @@ namespace gtsam {
* parents are already in the clique or its separators. This function does * parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures. * not check for this condition, it just updates the data structures.
*/ */
static void addToCliqueFront(BayesTree<CONDITIONAL>& bayesTree, static void addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree,
const sharedConditional& conditional, const sharedClique& clique); const sharedConditional& conditional, const sharedClique& clique);
/** Fill the nodes index for a subtree */ /** Fill the nodes index for a subtree */
@ -225,7 +137,7 @@ namespace gtsam {
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the * 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. * new root clique and the subtrees are connected to the root clique.
*/ */
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees); BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees);
/** Destructor */ /** Destructor */
virtual ~BayesTree() {} virtual ~BayesTree() {}
@ -238,7 +150,7 @@ namespace gtsam {
* This function only applies for Symbolic case with IndexCondtional, * This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case. * We make it static so that it won't be compiled in GaussianConditional case.
* */ * */
static void insert(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional); static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/** /**
* Insert a new clique corresponding to the given Bayes net. * Insert a new clique corresponding to the given Bayes net.
@ -262,7 +174,7 @@ namespace gtsam {
*/ */
/** check equality */ /** check equality */
bool equals(const BayesTree<CONDITIONAL>& other, double tol = 1e-9) const; bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
/** /**
* Find parent clique of a conditional. It will look at all parents and * Find parent clique of a conditional. It will look at all parents and
@ -357,10 +269,10 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
void _BayesTree_dim_adder( void _BayesTree_dim_adder(
std::vector<size_t>& dims, std::vector<size_t>& dims,
const typename BayesTree<CONDITIONAL>::sharedClique& clique) { const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) { if(clique) {
// Add dims from this clique // Add dims from this clique
@ -368,18 +280,133 @@ namespace gtsam {
dims[*it] = (*clique)->dim(it); dims[*it] = (*clique)->dim(it);
// Traverse children // Traverse children
BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, clique->children()) { typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
_BayesTree_dim_adder<CONDITIONAL>(dims, child); BOOST_FOREACH(const sharedClique& child, clique->children()) {
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL,class CLIQUE>
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL>& bt) { boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
std::vector<size_t> dimensions(bt.nodes().size(), 0); std::vector<size_t> dimensions(bt.nodes().size(), 0);
_BayesTree_dim_adder<CONDITIONAL>(dimensions, bt.root()); _BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions)); return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
} }
/* ************************************************************************* */
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*/
template<class CONDITIONAL>
struct BayesTreeClique {
protected:
void assertInvariants() const;
public:
typedef BayesTreeClique<CONDITIONAL> This;
typedef CONDITIONAL ConditionalType;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef typename boost::shared_ptr<This> shared_ptr;
typedef typename boost::weak_ptr<This> weak_ptr;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
sharedConditional conditional_;
weak_ptr parent_;
std::list<shared_ptr> children_;
friend class BayesTree<CONDITIONAL>;
/** Default constructor */
BayesTreeClique() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeClique(const sharedConditional& conditional);
virtual ~BayesTreeClique() {}
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */
static shared_ptr Create(const sharedConditional& conditional) { return shared_ptr(new BayesTreeClique(conditional)); }
/** Construct shared_ptr from a FactorGraph<FACTOR>::EliminationResult. In this class
* the conditional part is kept and the factor part is ignored, but in derived clique
* types, such as ISAM2Clique, the factor part is kept as a cached factor.
* @param
*/
template<class RESULT>
static shared_ptr Create(const RESULT& result) { return Create(result.first); }
/** print this node */
void print(const std::string& s = "") const;
/** The arrow operator accesses the conditional */
const CONDITIONAL* operator->() const { return conditional_.get(); }
/** The arrow operator accesses the conditional */
CONDITIONAL* operator->() { return conditional_.get(); }
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired(); }
/** return the const reference of children */
std::list<shared_ptr>& children() { return children_; }
const std::list<shared_ptr>& children() const { return children_; }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** print this node and entire subtree below it */
void printTree(const std::string& indent="") const;
/** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation);
/** Permute variables when they only appear in the separators. In this
* case the running intersection property will be used to prevent always
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version
BayesNet<CONDITIONAL> shortcut(shared_ptr root, Eliminate function);
/** return the marginal P(C) of the clique */
FactorGraph<FactorType> marginal(shared_ptr root, Eliminate function);
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<FactorType> joint(shared_ptr C2, shared_ptr root, Eliminate function);
bool equals(const This& other, double tol=1e-9) const {
return (!conditional_ && !other.conditional()) ||
conditional_->equals(*(other.conditional()), tol);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
}
}; // \struct Clique
} /// namespace gtsam } /// namespace gtsam

15
gtsam/inference/FactorGraph-inl.h
View File

@ -126,28 +126,29 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR, class CONDITIONAL> template<class FACTOR, class CONDITIONAL, class CLIQUE>
void _FactorGraph_BayesTree_adder( void _FactorGraph_BayesTree_adder(
vector<typename FactorGraph<FACTOR>::sharedFactor>& factors, vector<typename FactorGraph<FACTOR>::sharedFactor>& factors,
const typename BayesTree<CONDITIONAL>::sharedClique& clique) { const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) { if(clique) {
// Add factor from this clique // Add factor from this clique
factors.push_back((*clique)->toFactor()); factors.push_back((*clique)->toFactor());
// Traverse children // Traverse children
BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, clique->children()) { typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL>(factors, child); BOOST_FOREACH(const sharedClique& child, clique->children()) {
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors, child);
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL>& bayesTree) { FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL,CLIQUE>& bayesTree) {
factors_.reserve(bayesTree.size()); factors_.reserve(bayesTree.size());
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL>(factors_, bayesTree.root()); _FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_, bayesTree.root());
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/inference/FactorGraph.h
View File

@ -33,7 +33,7 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
template<class CONDITIONAL> class BayesTree; template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** /**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes. * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
@ -78,8 +78,8 @@ template<class CONDITIONAL> class BayesTree;
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet); FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
/** convert from Bayes net */ /** convert from Bayes net */
template<class CONDITIONAL> template<class CONDITIONAL, class CLIQUE>
FactorGraph(const BayesTree<CONDITIONAL>& bayesTree); FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
/** convert from a derived type */ /** convert from a derived type */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>

6
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -51,14 +51,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class F, class JT> template<class F, class JT>
typename JT::BayesTree::shared_ptr GenericMultifrontalSolver<F, JT>::eliminate( typename BayesTree<typename F::ConditionalType>::shared_ptr GenericMultifrontalSolver<F, JT>::eliminate(
typename FactorGraph<F>::Eliminate function) const { typename FactorGraph<F>::Eliminate function) const {
// eliminate junction tree, returns pointer to root // eliminate junction tree, returns pointer to root
typename JT::BayesTree::sharedClique root = junctionTree_->eliminate(function); typename BayesTree<typename F::ConditionalType>::sharedClique root = junctionTree_->eliminate(function);
// create an empty Bayes tree and insert root clique // create an empty Bayes tree and insert root clique
typename JT::BayesTree::shared_ptr bayesTree(new typename JT::BayesTree); typename BayesTree<typename F::ConditionalType>::shared_ptr bayesTree(new BayesTree<typename F::ConditionalType>);
bayesTree->insert(root); bayesTree->insert(root);
// return the Bayes tree // return the Bayes tree

2
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
typename JUNCTIONTREE::BayesTree::shared_ptr typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
eliminate(Eliminate function) const; eliminate(Eliminate function) const;
/** /**

56
gtsam/inference/JunctionTree-inl.h
View File

@ -36,8 +36,8 @@ namespace gtsam {
using namespace std; using namespace std;
/* ************************************************************************* */ /* ************************************************************************* */
template <class FG> template <class FG, class BTCLIQUE>
void JunctionTree<FG>::construct(const FG& fg, const VariableIndex& variableIndex) { void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {
tic(1, "JT Constructor"); tic(1, "JT Constructor");
tic(1, "JT symbolic ET"); tic(1, "JT symbolic ET");
const typename EliminationTree<IndexFactor>::shared_ptr symETree = const typename EliminationTree<IndexFactor>::shared_ptr symETree =
@ -58,8 +58,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class FG> template <class FG, class BTCLIQUE>
JunctionTree<FG>::JunctionTree(const FG& fg) { JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) {
tic(0, "VariableIndex"); tic(0, "VariableIndex");
VariableIndex varIndex(fg); VariableIndex varIndex(fg);
toc(0, "VariableIndex"); toc(0, "VariableIndex");
@ -67,14 +67,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class FG> template <class FG, class BTCLIQUE>
JunctionTree<FG>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) { JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
construct(fg, variableIndex); construct(fg, variableIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG, class BTCLIQUE>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors( typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(
const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) { const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) {
// Build "target" index. This is an index for each variable of the factors // Build "target" index. This is an index for each variable of the factors
@ -109,8 +109,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG, class BTCLIQUE>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(const FG& fg, typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
const std::vector<FastList<size_t> >& targets, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& bayesClique) { const SymbolicBayesTree::sharedClique& bayesClique) {
@ -147,21 +147,21 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG, class BTCLIQUE>
pair<typename JunctionTree<FG>::BayesTree::sharedClique, pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr,
typename FG::sharedFactor> JunctionTree<FG>::eliminateOneClique( typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique(
typename FG::Eliminate function, typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& current, bool cache) const { const boost::shared_ptr<const Clique>& current) const {
FG fg; // factor graph will be assembled from local factors and marginalized children FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size()); fg.reserve(current->size() + current->children().size());
fg.push_back(*current); // add the local factors fg.push_back(*current); // add the local factors
// receive the factors from the child and its clique point // receive the factors from the child and its clique point
list<typename BayesTree::sharedClique> children; list<typename BTClique::shared_ptr> children;
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) { BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor( pair<typename BTClique::shared_ptr, typename FG::sharedFactor> tree_factor(
eliminateOneClique(function, child, cache)); eliminateOneClique(function, child));
children.push_back(tree_factor.first); children.push_back(tree_factor.first);
fg.push_back(tree_factor.second); fg.push_back(tree_factor.second);
} }
@ -172,9 +172,7 @@ namespace gtsam {
// Now that we know which factors and variables, and where variables // Now that we know which factors and variables, and where variables
// come from and go to, create and eliminate the new joint factor. // come from and go to, create and eliminate the new joint factor.
tic(2, "CombineAndEliminate"); tic(2, "CombineAndEliminate");
pair< typename FG::EliminationResult eliminated(function(fg,
typename FG::FactorType::ConditionalType::shared_ptr,
typename FG::sharedFactor> eliminated(function(fg,
current->frontal.size())); current->frontal.size()));
toc(2, "CombineAndEliminate"); toc(2, "CombineAndEliminate");
@ -182,33 +180,31 @@ namespace gtsam {
tic(3, "Update tree"); tic(3, "Update tree");
// create a new clique corresponding the combined factors // create a new clique corresponding the combined factors
typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(eliminated.first)); typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated));
new_clique->children_ = children; new_clique->children_ = children;
BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children) { BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) {
childRoot->parent_ = new_clique; childRoot->parent_ = new_clique;
} }
if(cache)
new_clique->cachedFactor() = eliminated.second;
toc(3, "Update tree"); toc(3, "Update tree");
return make_pair(new_clique, eliminated.second); return make_pair(new_clique, eliminated.second);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG, class BTCLIQUE>
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate( typename BTCLIQUE::shared_ptr JunctionTree<FG,BTCLIQUE>::eliminate(
typename FG::Eliminate function, bool cache) const { typename FG::Eliminate function) const {
if (this->root()) { if (this->root()) {
tic(2, "JT eliminate"); tic(2, "JT eliminate");
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = pair<typename BTClique::shared_ptr, typename FG::sharedFactor> ret =
this->eliminateOneClique(function, this->root(), cache); this->eliminateOneClique(function, this->root());
if (ret.second->size() != 0) throw runtime_error( if (ret.second->size() != 0) throw runtime_error(
"JuntionTree::eliminate: elimination failed because of factors left over!"); "JuntionTree::eliminate: elimination failed because of factors left over!");
toc(2, "JT eliminate"); toc(2, "JT eliminate");
return ret.first; return ret.first;
} else } else
return typename BayesTree::sharedClique(); return typename BTClique::shared_ptr();
} }
} //namespace gtsam } //namespace gtsam

13
gtsam/inference/JunctionTree.h
View File

@ -45,7 +45,7 @@ namespace gtsam {
* *
* \ingroup Multifrontal * \ingroup Multifrontal
*/ */
template<class FG> template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
class JunctionTree: public ClusterTree<FG> { class JunctionTree: public ClusterTree<FG> {
public: public:
@ -55,7 +55,7 @@ namespace gtsam {
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
/// The BayesTree type produced by elimination /// The BayesTree type produced by elimination
typedef class BayesTree<typename FG::FactorType::ConditionalType> BayesTree; typedef BTCLIQUE BTClique;
/// Shared pointer to this class /// Shared pointer to this class
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr; typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
@ -73,9 +73,9 @@ namespace gtsam {
const SymbolicBayesTree::sharedClique& clique); const SymbolicBayesTree::sharedClique& clique);
// recursive elimination function // recursive elimination function
std::pair<typename BayesTree::sharedClique, typename FG::sharedFactor> std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
eliminateOneClique(typename FG::Eliminate function, eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique, bool cache = false) const; const boost::shared_ptr<const Clique>& clique) const;
// internal constructor // internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex); void construct(const FG& fg, const VariableIndex& variableIndex);
@ -103,12 +103,9 @@ namespace gtsam {
/** Eliminate the factors in the subgraphs to produce a BayesTree. /** Eliminate the factors in the subgraphs to produce a BayesTree.
* @param function The function used to eliminate, see the namespace functions * @param function The function used to eliminate, see the namespace functions
* in GaussianFactorGraph.h * in GaussianFactorGraph.h
* @param cache Whether to cache the intermediate elimination factors for use in ISAM2 - this
* should always be false when called outside of ISAM2 (this will be fixed in the future).
* @return The BayesTree resulting from elimination * @return The BayesTree resulting from elimination
*/ */
typename BayesTree::sharedClique eliminate(typename FG::Eliminate function, typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
bool cache = false) const;
}; // JunctionTree }; // JunctionTree

3
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -42,9 +42,6 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {} GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {}
/* ************************************************************************* */
GaussianFactorGraph::GaussianFactorGraph(const BayesTree<GaussianConditional>& GBT) : Base(GBT) {}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Index> keys; FastSet<Index> keys;

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -75,7 +75,8 @@ namespace gtsam {
/** /**
* Constructor that receives a BayesTree and returns a GaussianFactorGraph * Constructor that receives a BayesTree and returns a GaussianFactorGraph
*/ */
GaussianFactorGraph(const BayesTree<GaussianConditional>& GBT); template<class CLIQUE>
GaussianFactorGraph(const BayesTree<GaussianConditional,CLIQUE>& gbt) : Base(gbt) {}
/** Constructor from a factor graph of GaussianFactor or a derived type */ /** Constructor from a factor graph of GaussianFactor or a derived type */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>

10
gtsam/linear/GaussianJunctionTree.cpp
View File

@ -34,7 +34,7 @@ namespace gtsam {
using namespace std; using namespace std;
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const { void GaussianJunctionTree::btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const {
// solve the bayes net in the current node // solve the bayes net in the current node
current->conditional()->solveInPlace(config); current->conditional()->solveInPlace(config);
@ -47,15 +47,15 @@ namespace gtsam {
// } // }
// solve the bayes nets in the child nodes // solve the bayes nets in the child nodes
BOOST_FOREACH(const BayesTree::sharedClique& child, current->children()) { BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) {
btreeBackSubstitute(child, config); btreeBackSubstitute(child, config);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianJunctionTree::btreeRHS(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const { void GaussianJunctionTree::btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const {
current->conditional()->rhs(config); current->conditional()->rhs(config);
BOOST_FOREACH(const BayesTree::sharedClique& child, current->children()) BOOST_FOREACH(const BTClique::shared_ptr& child, current->children())
btreeRHS(child, config); btreeRHS(child, config);
} }
@ -63,7 +63,7 @@ namespace gtsam {
VectorValues GaussianJunctionTree::optimize(Eliminate function) const { VectorValues GaussianJunctionTree::optimize(Eliminate function) const {
tic(1, "GJT eliminate"); tic(1, "GJT eliminate");
// eliminate from leaves to the root // eliminate from leaves to the root
boost::shared_ptr<const BayesTree::Clique> rootClique(this->eliminate(function)); BTClique::shared_ptr rootClique(this->eliminate(function));
toc(1, "GJT eliminate"); toc(1, "GJT eliminate");
// Allocate solution vector and copy RHS // Allocate solution vector and copy RHS

14
gtsam/linear/GaussianJunctionTree.h
View File

@ -43,10 +43,10 @@ namespace gtsam {
protected: protected:
// back-substitute in topological sort order (parents first) // back-substitute in topological sort order (parents first)
void btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const; void btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const;
// find the RHS for the system in order to perform backsubstitution // find the RHS for the system in order to perform backsubstitution
void btreeRHS(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const; void btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const;
public : public :
@ -64,22 +64,22 @@ namespace gtsam {
VectorValues optimize(Eliminate function) const; VectorValues optimize(Eliminate function) const;
// convenient function to return dimensions of all variables in the BayesTree<GaussianConditional> // convenient function to return dimensions of all variables in the BayesTree<GaussianConditional>
template<class DIM_CONTAINER> template<class DIM_CONTAINER, class CLIQUE>
static void countDims(const BayesTree& bayesTree, DIM_CONTAINER& dims) { static void countDims(const BayesTree<GaussianConditional,CLIQUE>& bayesTree, DIM_CONTAINER& dims) {
dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0); dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0);
countDims(bayesTree.root(), dims); countDims(bayesTree.root(), dims);
} }
private: private:
template<class DIM_CONTAINER> template<class DIM_CONTAINER, class CLIQUE>
static void countDims(const boost::shared_ptr<const BayesTree::Clique>& clique, DIM_CONTAINER& dims) { static void countDims(const boost::shared_ptr<CLIQUE>& clique, DIM_CONTAINER& dims) {
GaussianConditional::const_iterator it = clique->conditional()->beginFrontals(); GaussianConditional::const_iterator it = clique->conditional()->beginFrontals();
for (; it != clique->conditional()->endFrontals(); ++it) { for (; it != clique->conditional()->endFrontals(); ++it) {
assert(dims.at(*it) == 0); assert(dims.at(*it) == 0);
dims.at(*it) = clique->conditional()->dim(it); dims.at(*it) = clique->conditional()->dim(it);
} }
BOOST_FOREACH(const boost::shared_ptr<const BayesTree::Clique>& child, clique->children()) { BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children()) {
countDims(child, dims); countDims(child, dims);
} }
} }

12
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -85,7 +85,7 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
* *
* Alternatively could we trace up towards the root for each variable here? * Alternatively could we trace up towards the root for each variable here?
*/ */
static void FindAll(ISAM2Type::sharedClique clique, FastSet<Index>& keys, const vector<bool>& markedMask); static void FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
/** /**
* Apply expmap to the given values, but only for indices appearing in * Apply expmap to the given values, but only for indices appearing in
@ -191,8 +191,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<Ve
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Values> template<class CONDITIONAL, class VALUES>
void ISAM2<Conditional, Values>::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet<Index>& keys, const vector<bool>& markedMask) { void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false; static const bool debug = false;
// does the separator contain any of the variables? // does the separator contain any of the variables?
bool found = false; bool found = false;
@ -206,7 +206,7 @@ void ISAM2<Conditional, Values>::Impl::FindAll(ISAM2Type::sharedClique clique, F
if(debug) clique->print("Key(s) marked in clique "); if(debug) clique->print("Key(s) marked in clique ");
if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
} }
BOOST_FOREACH(const sharedClique& child, clique->children_) { BOOST_FOREACH(const typename BayesTreeClique<CONDITIONAL>::shared_ptr& child, clique->children_) {
FindAll(child, keys, markedMask); FindAll(child, keys, markedMask);
} }
} }
@ -338,8 +338,8 @@ ISAM2<CONDITIONAL, VALUES>::Impl::PartialSolve(GaussianFactorGraph& factors,
// eliminate into a Bayes net // eliminate into a Bayes net
tic(7,"eliminate"); tic(7,"eliminate");
GaussianJunctionTree jt(factors, affectedFactorsIndex); JunctionTree<GaussianFactorGraph, typename ISAM2Type::Clique> jt(factors, affectedFactorsIndex);
result.bayesTree = jt.eliminate(EliminatePreferLDL, true); result.bayesTree = jt.eliminate(EliminatePreferLDL);
if(debug && result.bayesTree) { if(debug && result.bayesTree) {
cout << "Re-eliminated BT:\n"; cout << "Re-eliminated BT:\n";
result.bayesTree->printTree(""); result.bayesTree->printTree("");

14
gtsam/nonlinear/ISAM2-inl.h
View File

@ -192,7 +192,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
sharedClique clique = this->nodes_[key]; sharedClique clique = this->nodes_[key];
while(clique) { while(clique) {
affectedStructuralKeys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); affectedStructuralKeys.insert((*clique)->beginFrontals(), (*clique)->endFrontals());
clique = clique->parent_.lock(); #ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique
clique = boost::dynamic_pointer_cast<Clique>(clique->parent_.lock());
#else
clique = boost::static_pointer_cast<Clique>(clique->parent_.lock());
#endif
} }
} }
toc(0, "affectedStructuralKeys"); toc(0, "affectedStructuralKeys");
@ -286,13 +290,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
toc(2,"linearize"); toc(2,"linearize");
tic(5,"eliminate"); tic(5,"eliminate");
GaussianJunctionTree jt(factors, variableIndex_); JunctionTree<GaussianFactorGraph, typename Base::Clique> jt(factors, variableIndex_);
sharedClique newRoot = jt.eliminate(EliminatePreferLDL, true); sharedClique newRoot = jt.eliminate(EliminatePreferLDL);
if(debug) newRoot->print("Eliminated: "); if(debug) newRoot->print("Eliminated: ");
toc(5,"eliminate"); toc(5,"eliminate");
tic(6,"insert"); tic(6,"insert");
BayesTree<Conditional>::clear(); Base::clear();
this->insert(newRoot); this->insert(newRoot);
toc(6,"insert"); toc(6,"insert");
@ -550,6 +554,8 @@ ISAM2Result ISAM2<Conditional, Values>::update(
result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
toc(10,"evaluate error after"); toc(10,"evaluate error after");
result.cliques = this->nodes().size();
return result; return result;
} }

90
gtsam/nonlinear/ISAM2.h
View File

@ -113,6 +113,87 @@ struct ISAM2Result {
* will be reeliminated. * will be reeliminated.
*/ */
size_t variablesReeliminated; size_t variablesReeliminated;
/** The number of cliques in the Bayes' Tree */
size_t cliques;
};
template<class CONDITIONAL>
struct ISAM2Clique : public BayesTreeClique<CONDITIONAL> {
typedef ISAM2Clique<CONDITIONAL> This;
typedef BayesTreeClique<CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typename Base::FactorType::shared_ptr cachedFactor_;
/** Access the cached factor */
typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** Construct from an elimination result, caches the eliminated factor */
template<class RESULT>
ISAM2Clique(const RESULT& result) : Base(result), cachedFactor_(result.second) {}
/** Construct from a conditional */
ISAM2Clique(const typename Base::sharedConditional& conditional) : Base(conditional) {}
/** Create from an elimination result, overrides BayesTreeClique<CONDITIONAL>::Create(const RESULT&) to cache the eliminated factor */
template<class RESULT>
static shared_ptr Create(const RESULT& result) { return shared_ptr(new This(result)); }
static shared_ptr Create(const typename Base::sharedConditional& conditional) { return shared_ptr(new This(conditional)); }
void permuteWithInverse(const Permutation& inversePermutation) {
Base::conditional_->permuteWithInverse(inversePermutation);
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) {
shared_ptr _child;
#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique
_child = boost::dynamic_pointer_cast<This>(child);
#else
_child = boost::static_pointer_cast<This>(child);
#endif
_child->permuteWithInverse(inversePermutation);
}
Base::assertInvariants();
}
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = Base::conditional_->permuteSeparatorWithInverse(inversePermutation);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(Index& separatorKey, Base::conditional_->parents()) {
assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) {
shared_ptr _child = boost::dynamic_pointer_cast<This>(child); // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique
assert(_child->permuteSeparatorWithInverse(inversePermutation) == false); }
}
#endif
if(changed) {
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) {
shared_ptr _child;
#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique
_child = boost::dynamic_pointer_cast<This>(child);
#else
_child = boost::static_pointer_cast<Clique>(child);
#endif
(void)_child->permuteSeparatorWithInverse(inversePermutation);
}
}
Base::assertInvariants();
return changed;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
}
}; };
/** /**
@ -125,7 +206,7 @@ struct ISAM2Result {
* estimate of all variables. * estimate of all variables.
*/ */
template<class CONDITIONAL, class VALUES> template<class CONDITIONAL, class VALUES>
class ISAM2: public BayesTree<CONDITIONAL> { class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
protected: protected:
@ -168,7 +249,7 @@ private:
public: public:
typedef BayesTree<CONDITIONAL> Base; ///< The BayesTree base class typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
/** Create an empty ISAM2 instance */ /** Create an empty ISAM2 instance */
@ -177,8 +258,9 @@ public:
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
ISAM2(); ISAM2();
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename Base::Clique Clique; ///< A clique
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///< List of Clique typedef from base class typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class
/** /**
* Add new factors, updating the solution and relinearizing as needed. * Add new factors, updating the solution and relinearizing as needed.

2
tests/testGaussianISAM2.cpp
View File

@ -167,7 +167,7 @@ TEST(ISAM2, optimize2) {
conditional->solveInPlace(expected); conditional->solveInPlace(expected);
// Clique // Clique
GaussianISAM2<planarSLAM::Values>::sharedClique clique(new GaussianISAM2<planarSLAM::Values>::Clique(conditional)); GaussianISAM2<planarSLAM::Values>::sharedClique clique(GaussianISAM2<planarSLAM::Values>::Clique::Create(conditional));
VectorValues actual(theta.dims(ordering)); VectorValues actual(theta.dims(ordering));
conditional->rhs(actual); conditional->rhs(actual);
optimize2(clique, actual); optimize2(clique, actual);