(in branch) Dogleg in ISAM2 in progress
Richard Roberts
parent
e78668bad9
commit
e6a43d6330
|
|
@ -44,7 +44,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::Clique::assertInvariants() const {
|
||||
void BayesTreeClique<CONDITIONAL>::assertInvariants() const {
|
||||
#ifndef NDEBUG
|
||||
// We rely on the keys being sorted
|
||||
// FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
|
||||
|
|
@ -57,19 +57,19 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) : conditional_(conditional) {
|
||||
BayesTreeClique<CONDITIONAL>::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::Clique::print(const string& s) const {
|
||||
void BayesTreeClique<CONDITIONAL>::print(const string& s) const {
|
||||
conditional_->print(s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
size_t BayesTree<CONDITIONAL>::Clique::treeSize() const {
|
||||
size_t BayesTreeClique<CONDITIONAL>::treeSize() const {
|
||||
size_t size = 1;
|
||||
BOOST_FOREACH(const shared_ptr& child, children_)
|
||||
size += child->treeSize();
|
||||
|
|
@ -78,7 +78,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::Clique::printTree(const string& indent) const {
|
||||
void BayesTreeClique<CONDITIONAL>::printTree(const string& indent) const {
|
||||
print(indent);
|
||||
BOOST_FOREACH(const shared_ptr& child, children_)
|
||||
child->printTree(indent+" ");
|
||||
|
|
@ -86,9 +86,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::Clique::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
void BayesTreeClique<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
conditional_->permuteWithInverse(inversePermutation);
|
||||
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
|
||||
BOOST_FOREACH(const shared_ptr& child, children_) {
|
||||
child->permuteWithInverse(inversePermutation);
|
||||
}
|
||||
|
|
@ -97,7 +96,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
bool BayesTree<CONDITIONAL>::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
|
||||
bool BayesTreeClique<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
|
||||
bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation);
|
||||
#ifndef NDEBUG
|
||||
if(!changed) {
|
||||
|
|
@ -108,7 +107,6 @@ namespace gtsam {
|
|||
}
|
||||
#endif
|
||||
if(changed) {
|
||||
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
|
||||
BOOST_FOREACH(const shared_ptr& child, children_) {
|
||||
(void)child->permuteSeparatorWithInverse(inversePermutation);
|
||||
}
|
||||
|
|
@ -118,17 +116,17 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesTree<CONDITIONAL>::CliqueData
|
||||
BayesTree<CONDITIONAL>::getCliqueData() const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
|
||||
BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const {
|
||||
CliqueData data;
|
||||
getCliqueData(data, root_);
|
||||
return data;
|
||||
}
|
||||
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::getCliqueData(CliqueData& data,
|
||||
BayesTree<CONDITIONAL>::sharedClique clique) const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data,
|
||||
BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique) const {
|
||||
data.conditionalSizes.push_back((*clique)->nrFrontals());
|
||||
data.separatorSizes.push_back((*clique)->nrParents());
|
||||
BOOST_FOREACH(sharedClique c, clique->children_) {
|
||||
|
|
@ -137,8 +135,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::saveGraph(const std::string &s) const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
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!");
|
||||
ofstream of(s.c_str());
|
||||
of<< "digraph G{\n";
|
||||
|
|
@ -147,9 +145,9 @@ namespace gtsam {
|
|||
of.close();
|
||||
}
|
||||
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::saveGraph(ostream &s,
|
||||
BayesTree<CONDITIONAL>::sharedClique clique,
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s,
|
||||
BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique,
|
||||
int parentnum) const {
|
||||
static int num = 0;
|
||||
bool first = true;
|
||||
|
|
@ -184,9 +182,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
|
||||
template<class CONDITIONAL>
|
||||
typename BayesTree<CONDITIONAL>::CliqueStats
|
||||
BayesTree<CONDITIONAL>::CliqueData::getStats() const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
|
||||
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
|
||||
CliqueStats stats;
|
||||
|
||||
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
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R,
|
||||
BayesNet<CONDITIONAL> BayesTreeClique<CONDITIONAL>::shortcut(shared_ptr R,
|
||||
Eliminate function) {
|
||||
|
||||
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,
|
||||
// in which case we return an empty Bayes net.
|
||||
|
||||
sharedClique parent(parent_.lock());
|
||||
shared_ptr parent(parent_.lock());
|
||||
|
||||
if (R.get()==this || parent==R) {
|
||||
BayesNet<CONDITIONAL> empty;
|
||||
|
|
@ -318,7 +316,7 @@ namespace gtsam {
|
|||
// Because the root clique could be very big.
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::marginal(
|
||||
FactorGraph<typename CONDITIONAL::FactorType> BayesTreeClique<CONDITIONAL>::marginal(
|
||||
shared_ptr R, Eliminate function) {
|
||||
// If we are the root, just return this root
|
||||
// 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)
|
||||
/* ************************************************************************* */
|
||||
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) {
|
||||
// For now, assume neither is the root
|
||||
|
||||
|
|
@ -367,23 +365,23 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::Cliques::print(const std::string& s) const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s) const {
|
||||
cout << s << ":\n";
|
||||
BOOST_FOREACH(sharedClique clique, *this)
|
||||
clique->printTree();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
bool BayesTree<CONDITIONAL>::Cliques::equals(const Cliques& other, double tol) const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
|
||||
return other == *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique(
|
||||
const sharedConditional& conditional, sharedClique parent_clique) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique
|
||||
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, sharedClique parent_clique) {
|
||||
sharedClique new_clique(new Clique(conditional));
|
||||
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
|
||||
BOOST_FOREACH(Index key, conditional->frontals())
|
||||
|
|
@ -397,8 +395,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique(
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
|
||||
const sharedConditional& conditional, list<sharedClique>& child_cliques) {
|
||||
sharedClique new_clique(new Clique(conditional));
|
||||
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
|
||||
|
|
@ -412,8 +410,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
inline void BayesTree<CONDITIONAL>::addToCliqueFront(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
|
||||
static const bool debug = false;
|
||||
#ifndef NDEBUG
|
||||
// Debug check to make sure the conditional variable is ordered lower than
|
||||
|
|
@ -439,8 +437,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::removeClique(sharedClique clique) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
|
||||
|
||||
if (clique->isRoot())
|
||||
root_.reset();
|
||||
|
|
@ -449,7 +447,7 @@ namespace gtsam {
|
|||
|
||||
// orphan my 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())) {
|
||||
nodes_[key].reset();
|
||||
|
|
@ -457,27 +455,28 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
BayesTree<CONDITIONAL>::BayesTree() {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree() {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||
insert(*this, *rit);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > 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) {
|
||||
typedef BayesTree<CONDITIONAL,CLIQUE> Tree;
|
||||
BOOST_FOREACH(const Tree& subtree, subtrees) {
|
||||
nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end());
|
||||
childRoots.push_back(subtree.root());
|
||||
}
|
||||
|
|
@ -496,8 +495,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::print(const string& s) const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::print(const string& s) const {
|
||||
if (root_.use_count() == 0) {
|
||||
printf("WARNING: BayesTree.print encountered a forest...\n");
|
||||
return;
|
||||
|
|
@ -509,26 +508,26 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// binary predicate to test equality of a pair for use in equals
|
||||
template<class CONDITIONAL>
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool check_sharedCliques(
|
||||
const typename BayesTree<CONDITIONAL>::sharedClique& v1,
|
||||
const typename BayesTree<CONDITIONAL>::sharedClique& v2
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2
|
||||
) {
|
||||
return v1->equals(*v2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
bool BayesTree<CONDITIONAL>::equals(const BayesTree<CONDITIONAL>& other,
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool BayesTree<CONDITIONAL,CLIQUE>::equals(const BayesTree<CONDITIONAL,CLIQUE>& other,
|
||||
double tol) const {
|
||||
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>
|
||||
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());
|
||||
assert(lowestOrderedParent != parents.end());
|
||||
return *lowestOrderedParent;
|
||||
|
|
@ -548,8 +547,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::insert(BayesTree<CONDITIONAL>& bayesTree, const sharedConditional& conditional)
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional)
|
||||
{
|
||||
static const bool debug = false;
|
||||
|
||||
|
|
@ -597,8 +596,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
//TODO: remove this function after removing TSAM.cpp
|
||||
template<class CONDITIONAL>
|
||||
typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::insert(
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert(
|
||||
const sharedConditional& clique, list<sharedClique>& children, bool isRootClique) {
|
||||
|
||||
if (clique->nrFrontals() == 0)
|
||||
|
|
@ -612,18 +611,19 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::fillNodesIndex(const sharedClique& subtree) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
|
||||
// Add each frontal variable of this root node
|
||||
BOOST_FOREACH(const Index& key, subtree->conditional()->frontals()) { nodes_[key] = subtree; }
|
||||
// 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); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::insert(const sharedClique& subtree) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
|
||||
if(subtree) {
|
||||
// Find the parent clique of the new subtree. By the running intersection
|
||||
// property, those separator variables in the subtree that are ordered
|
||||
|
|
@ -651,8 +651,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// First finds clique marginal then marginalizes that
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL>::marginalFactor(
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor(
|
||||
Index key, Eliminate function) const {
|
||||
|
||||
// get clique containing key
|
||||
|
|
@ -666,8 +666,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet(
|
||||
Index key, Eliminate function) const {
|
||||
|
||||
// calculate marginal as a factor graph
|
||||
|
|
@ -681,9 +681,9 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// Find two cliques, their joint, then marginalizes
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr BayesTree<
|
||||
CONDITIONAL>::joint(Index key1, Index key2, Eliminate function) const {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
|
||||
BayesTree<CONDITIONAL,CLIQUE>::joint(Index key1, Index key2, Eliminate function) const {
|
||||
|
||||
// get clique C1 and C2
|
||||
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
|
||||
|
|
@ -698,8 +698,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet(
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet(
|
||||
Index key1, Index key2, Eliminate function) const {
|
||||
|
||||
// eliminate factor graph marginal to a Bayes net
|
||||
|
|
@ -708,17 +708,17 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::clear() {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::clear() {
|
||||
// Remove all nodes and clear the root pointer
|
||||
nodes_.clear();
|
||||
root_.reset();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesTree<CONDITIONAL>::removePath(sharedClique clique,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
|
||||
|
||||
// base case is NULL, if so we do nothing and return empties above
|
||||
if (clique!=NULL) {
|
||||
|
|
@ -730,7 +730,7 @@ namespace gtsam {
|
|||
this->removeClique(clique);
|
||||
|
||||
// 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_)
|
||||
orphans.splice (orphans.begin(), clique->children_);
|
||||
|
|
@ -741,10 +741,10 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
template<class CONTAINER>
|
||||
void BayesTree<CONDITIONAL>::removeTop(const CONTAINER& keys,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) {
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
BOOST_FOREACH(const Index& key, keys) {
|
||||
|
|
|
|||
|
|
@ -33,14 +33,19 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
|
||||
template<class CONDITIONAL> class BayesTreeClique;
|
||||
|
||||
/**
|
||||
* Bayes tree
|
||||
* Templated on the CONDITIONAL class, the type of node in the underlying Bayes chain.
|
||||
* This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional
|
||||
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
|
||||
* 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
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
|
||||
class BayesTree {
|
||||
|
||||
public:
|
||||
|
|
@ -52,100 +57,7 @@ namespace gtsam {
|
|||
typedef typename CONDITIONAL::FactorType FactorType;
|
||||
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
|
||||
|
||||
/**
|
||||
* 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 CLIQUE Clique; ///< The clique type, normally BayesTreeClique
|
||||
|
||||
// typedef for shared pointers to cliques
|
||||
typedef boost::shared_ptr<Clique> sharedClique;
|
||||
|
|
@ -207,7 +119,7 @@ namespace gtsam {
|
|||
* parents are already in the clique or its separators. This function does
|
||||
* 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);
|
||||
|
||||
/** 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
|
||||
* 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 */
|
||||
virtual ~BayesTree() {}
|
||||
|
|
@ -238,7 +150,7 @@ namespace gtsam {
|
|||
* This function only applies for Symbolic case with IndexCondtional,
|
||||
* 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.
|
||||
|
|
@ -262,7 +174,7 @@ namespace gtsam {
|
|||
*/
|
||||
|
||||
/** 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
|
||||
|
|
@ -357,10 +269,10 @@ namespace gtsam {
|
|||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void _BayesTree_dim_adder(
|
||||
std::vector<size_t>& dims,
|
||||
const typename BayesTree<CONDITIONAL>::sharedClique& clique) {
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
// Add dims from this clique
|
||||
|
|
@ -368,18 +280,133 @@ namespace gtsam {
|
|||
dims[*it] = (*clique)->dim(it);
|
||||
|
||||
// Traverse children
|
||||
BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, clique->children()) {
|
||||
_BayesTree_dim_adder<CONDITIONAL>(dims, child);
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children()) {
|
||||
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL>& bt) {
|
||||
template<class CONDITIONAL,class CLIQUE>
|
||||
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
|
||||
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));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -126,28 +126,29 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR, class CONDITIONAL>
|
||||
template<class FACTOR, class CONDITIONAL, class CLIQUE>
|
||||
void _FactorGraph_BayesTree_adder(
|
||||
vector<typename FactorGraph<FACTOR>::sharedFactor>& factors,
|
||||
const typename BayesTree<CONDITIONAL>::sharedClique& clique) {
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
// Add factor from this clique
|
||||
factors.push_back((*clique)->toFactor());
|
||||
|
||||
// Traverse children
|
||||
BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, clique->children()) {
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL>(factors, child);
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children()) {
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL>& bayesTree) {
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL,CLIQUE>& bayesTree) {
|
||||
factors_.reserve(bayesTree.size());
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL>(factors_, bayesTree.root());
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_, bayesTree.root());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
namespace gtsam {
|
||||
|
||||
// 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.
|
||||
|
|
@ -78,8 +78,8 @@ template<class CONDITIONAL> class BayesTree;
|
|||
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
|
||||
/** convert from Bayes net */
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph(const BayesTree<CONDITIONAL>& bayesTree);
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
|
||||
|
||||
/** convert from a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
|
|
|
|||
|
|
@ -51,14 +51,14 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 {
|
||||
|
||||
// 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
|
||||
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);
|
||||
|
||||
// return the Bayes tree
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ namespace gtsam {
|
|||
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
||||
* to recursively eliminate.
|
||||
*/
|
||||
typename JUNCTIONTREE::BayesTree::shared_ptr
|
||||
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
|
||||
eliminate(Eliminate function) const;
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -36,8 +36,8 @@ namespace gtsam {
|
|||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG>
|
||||
void JunctionTree<FG>::construct(const FG& fg, const VariableIndex& variableIndex) {
|
||||
template <class FG, class BTCLIQUE>
|
||||
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {
|
||||
tic(1, "JT Constructor");
|
||||
tic(1, "JT symbolic ET");
|
||||
const typename EliminationTree<IndexFactor>::shared_ptr symETree =
|
||||
|
|
@ -58,8 +58,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG>
|
||||
JunctionTree<FG>::JunctionTree(const FG& fg) {
|
||||
template <class FG, class BTCLIQUE>
|
||||
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) {
|
||||
tic(0, "VariableIndex");
|
||||
VariableIndex varIndex(fg);
|
||||
toc(0, "VariableIndex");
|
||||
|
|
@ -67,14 +67,14 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG>
|
||||
JunctionTree<FG>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
|
||||
template <class FG, class BTCLIQUE>
|
||||
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
|
||||
construct(fg, variableIndex);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(
|
||||
const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) {
|
||||
|
||||
// Build "target" index. This is an index for each variable of the factors
|
||||
|
|
@ -109,8 +109,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(const FG& fg,
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
|
||||
const std::vector<FastList<size_t> >& targets,
|
||||
const SymbolicBayesTree::sharedClique& bayesClique) {
|
||||
|
||||
|
|
@ -147,21 +147,21 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
pair<typename JunctionTree<FG>::BayesTree::sharedClique,
|
||||
typename FG::sharedFactor> JunctionTree<FG>::eliminateOneClique(
|
||||
template<class FG, class BTCLIQUE>
|
||||
pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr,
|
||||
typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique(
|
||||
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.reserve(current->size() + current->children().size());
|
||||
fg.push_back(*current); // add the local factors
|
||||
|
||||
// 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()) {
|
||||
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
|
||||
eliminateOneClique(function, child, cache));
|
||||
pair<typename BTClique::shared_ptr, typename FG::sharedFactor> tree_factor(
|
||||
eliminateOneClique(function, child));
|
||||
children.push_back(tree_factor.first);
|
||||
fg.push_back(tree_factor.second);
|
||||
}
|
||||
|
|
@ -172,9 +172,7 @@ namespace gtsam {
|
|||
// Now that we know which factors and variables, and where variables
|
||||
// come from and go to, create and eliminate the new joint factor.
|
||||
tic(2, "CombineAndEliminate");
|
||||
pair<
|
||||
typename FG::FactorType::ConditionalType::shared_ptr,
|
||||
typename FG::sharedFactor> eliminated(function(fg,
|
||||
typename FG::EliminationResult eliminated(function(fg,
|
||||
current->frontal.size()));
|
||||
toc(2, "CombineAndEliminate");
|
||||
|
||||
|
|
@ -182,33 +180,31 @@ namespace gtsam {
|
|||
|
||||
tic(3, "Update tree");
|
||||
// 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;
|
||||
|
||||
BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children) {
|
||||
BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) {
|
||||
childRoot->parent_ = new_clique;
|
||||
}
|
||||
if(cache)
|
||||
new_clique->cachedFactor() = eliminated.second;
|
||||
toc(3, "Update tree");
|
||||
|
||||
return make_pair(new_clique, eliminated.second);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate(
|
||||
typename FG::Eliminate function, bool cache) const {
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename BTCLIQUE::shared_ptr JunctionTree<FG,BTCLIQUE>::eliminate(
|
||||
typename FG::Eliminate function) const {
|
||||
if (this->root()) {
|
||||
tic(2, "JT eliminate");
|
||||
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret =
|
||||
this->eliminateOneClique(function, this->root(), cache);
|
||||
pair<typename BTClique::shared_ptr, typename FG::sharedFactor> ret =
|
||||
this->eliminateOneClique(function, this->root());
|
||||
if (ret.second->size() != 0) throw runtime_error(
|
||||
"JuntionTree::eliminate: elimination failed because of factors left over!");
|
||||
toc(2, "JT eliminate");
|
||||
return ret.first;
|
||||
} else
|
||||
return typename BayesTree::sharedClique();
|
||||
return typename BTClique::shared_ptr();
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ namespace gtsam {
|
|||
*
|
||||
* \ingroup Multifrontal
|
||||
*/
|
||||
template<class FG>
|
||||
template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
|
||||
class JunctionTree: public ClusterTree<FG> {
|
||||
|
||||
public:
|
||||
|
|
@ -55,7 +55,7 @@ namespace gtsam {
|
|||
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
|
||||
|
||||
/// The BayesTree type produced by elimination
|
||||
typedef class BayesTree<typename FG::FactorType::ConditionalType> BayesTree;
|
||||
typedef BTCLIQUE BTClique;
|
||||
|
||||
/// Shared pointer to this class
|
||||
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
|
||||
|
|
@ -73,9 +73,9 @@ namespace gtsam {
|
|||
const SymbolicBayesTree::sharedClique& clique);
|
||||
|
||||
// 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,
|
||||
const boost::shared_ptr<const Clique>& clique, bool cache = false) const;
|
||||
const boost::shared_ptr<const Clique>& clique) const;
|
||||
|
||||
// internal constructor
|
||||
void construct(const FG& fg, const VariableIndex& variableIndex);
|
||||
|
|
@ -103,12 +103,9 @@ namespace gtsam {
|
|||
/** Eliminate the factors in the subgraphs to produce a BayesTree.
|
||||
* @param function The function used to eliminate, see the namespace functions
|
||||
* 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
|
||||
*/
|
||||
typename BayesTree::sharedClique eliminate(typename FG::Eliminate function,
|
||||
bool cache = false) const;
|
||||
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
|
||||
|
||||
}; // JunctionTree
|
||||
|
||||
|
|
|
|||
|
|
@ -42,9 +42,6 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const BayesTree<GaussianConditional>& GBT) : Base(GBT) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
|
||||
FastSet<Index> keys;
|
||||
|
|
|
|||
|
|
@ -75,7 +75,8 @@ namespace gtsam {
|
|||
/**
|
||||
* 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 */
|
||||
template<class DERIVEDFACTOR>
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ namespace gtsam {
|
|||
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
|
||||
current->conditional()->solveInPlace(config);
|
||||
|
||||
|
|
@ -47,15 +47,15 @@ namespace gtsam {
|
|||
// }
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
BOOST_FOREACH(const BayesTree::sharedClique& child, current->children())
|
||||
BOOST_FOREACH(const BTClique::shared_ptr& child, current->children())
|
||||
btreeRHS(child, config);
|
||||
}
|
||||
|
||||
|
|
@ -63,7 +63,7 @@ namespace gtsam {
|
|||
VectorValues GaussianJunctionTree::optimize(Eliminate function) const {
|
||||
tic(1, "GJT eliminate");
|
||||
// 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");
|
||||
|
||||
// Allocate solution vector and copy RHS
|
||||
|
|
|
|||
|
|
@ -43,10 +43,10 @@ namespace gtsam {
|
|||
|
||||
protected:
|
||||
// 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
|
||||
void btreeRHS(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const;
|
||||
void btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const;
|
||||
|
||||
public :
|
||||
|
||||
|
|
@ -64,22 +64,22 @@ namespace gtsam {
|
|||
VectorValues optimize(Eliminate function) const;
|
||||
|
||||
// convenient function to return dimensions of all variables in the BayesTree<GaussianConditional>
|
||||
template<class DIM_CONTAINER>
|
||||
static void countDims(const BayesTree& bayesTree, DIM_CONTAINER& dims) {
|
||||
template<class DIM_CONTAINER, class CLIQUE>
|
||||
static void countDims(const BayesTree<GaussianConditional,CLIQUE>& bayesTree, DIM_CONTAINER& dims) {
|
||||
dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0);
|
||||
countDims(bayesTree.root(), dims);
|
||||
}
|
||||
|
||||
private:
|
||||
template<class DIM_CONTAINER>
|
||||
static void countDims(const boost::shared_ptr<const BayesTree::Clique>& clique, DIM_CONTAINER& dims) {
|
||||
template<class DIM_CONTAINER, class CLIQUE>
|
||||
static void countDims(const boost::shared_ptr<CLIQUE>& clique, DIM_CONTAINER& dims) {
|
||||
GaussianConditional::const_iterator it = clique->conditional()->beginFrontals();
|
||||
for (; it != clique->conditional()->endFrontals(); ++it) {
|
||||
assert(dims.at(*it) == 0);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
|
|||
*
|
||||
* 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
|
||||
|
|
@ -191,8 +191,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<Ve
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
void ISAM2<Conditional, Values>::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
static const bool debug = false;
|
||||
// does the separator contain any of the variables?
|
||||
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) 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -338,8 +338,8 @@ ISAM2<CONDITIONAL, VALUES>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|||
|
||||
// eliminate into a Bayes net
|
||||
tic(7,"eliminate");
|
||||
GaussianJunctionTree jt(factors, affectedFactorsIndex);
|
||||
result.bayesTree = jt.eliminate(EliminatePreferLDL, true);
|
||||
JunctionTree<GaussianFactorGraph, typename ISAM2Type::Clique> jt(factors, affectedFactorsIndex);
|
||||
result.bayesTree = jt.eliminate(EliminatePreferLDL);
|
||||
if(debug && result.bayesTree) {
|
||||
cout << "Re-eliminated BT:\n";
|
||||
result.bayesTree->printTree("");
|
||||
|
|
|
|||
|
|
@ -192,7 +192,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
|
|||
sharedClique clique = this->nodes_[key];
|
||||
while(clique) {
|
||||
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");
|
||||
|
|
@ -286,13 +290,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
|
|||
toc(2,"linearize");
|
||||
|
||||
tic(5,"eliminate");
|
||||
GaussianJunctionTree jt(factors, variableIndex_);
|
||||
sharedClique newRoot = jt.eliminate(EliminatePreferLDL, true);
|
||||
JunctionTree<GaussianFactorGraph, typename Base::Clique> jt(factors, variableIndex_);
|
||||
sharedClique newRoot = jt.eliminate(EliminatePreferLDL);
|
||||
if(debug) newRoot->print("Eliminated: ");
|
||||
toc(5,"eliminate");
|
||||
|
||||
tic(6,"insert");
|
||||
BayesTree<Conditional>::clear();
|
||||
Base::clear();
|
||||
this->insert(newRoot);
|
||||
toc(6,"insert");
|
||||
|
||||
|
|
@ -550,6 +554,8 @@ ISAM2Result ISAM2<Conditional, Values>::update(
|
|||
result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
|
||||
toc(10,"evaluate error after");
|
||||
|
||||
result.cliques = this->nodes().size();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -113,6 +113,87 @@ struct ISAM2Result {
|
|||
* will be reeliminated.
|
||||
*/
|
||||
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.
|
||||
*/
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
class ISAM2: public BayesTree<CONDITIONAL> {
|
||||
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
|
||||
|
||||
protected:
|
||||
|
||||
|
|
@ -168,7 +249,7 @@ private:
|
|||
|
||||
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
|
||||
|
||||
/** Create an empty ISAM2 instance */
|
||||
|
|
@ -177,8 +258,9 @@ public:
|
|||
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
|
||||
ISAM2();
|
||||
|
||||
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///< Shared pointer to a clique
|
||||
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///< List of Clique typedef from base class
|
||||
typedef typename Base::Clique Clique; ///< A clique
|
||||
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.
|
||||
|
|
|
|||
|
|
@ -167,7 +167,7 @@ TEST(ISAM2, optimize2) {
|
|||
conditional->solveInPlace(expected);
|
||||
|
||||
// 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));
|
||||
conditional->rhs(actual);
|
||||
optimize2(clique, actual);
|
||||
|
|
|
|||
Loading…
Reference in New Issue