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

(in branch) in progress refactoring for incremental dogleg

release/4.3a0
Richard Roberts 2011-12-05 02:28:09 +00:00
parent a0abe68b64
commit fd5b040385
8 changed files with 451 additions and 409 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -42,79 +42,6 @@ namespace gtsam {
using namespace std;
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::assertInvariants() const {
#ifndef NDEBUG
// We rely on the keys being sorted
// FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end());
// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end());
// assert(sortedUniqueKeys.size() == conditional_->size() &&
// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin()));
#endif
}
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTreeClique<CONDITIONAL>::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) {
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::print(const string& s) const {
conditional_->print(s);
}
/* ************************************************************************* */
template<class CONDITIONAL>
size_t BayesTreeClique<CONDITIONAL>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const shared_ptr& child, children_)
size += child->treeSize();
return size;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::printTree(const string& indent) const {
print(indent);
BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent+" ");
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
}
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesTreeClique<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(const shared_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
}
#endif
if(changed) {
BOOST_FOREACH(const shared_ptr& child, children_) {
(void)child->permuteSeparatorWithInverse(inversePermutation);
}
}
assertInvariants();
return changed;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
@ -208,162 +135,6 @@ namespace gtsam {
return stats;
}
/* ************************************************************************* */
// The shortcut density is a conditional P(S|R) of the separator of this
// clique on the root. We can compute it recursively from the parent shortcut
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
/* ************************************************************************* */
template<class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTreeClique<CONDITIONAL>::shortcut(shared_ptr R,
Eliminate function) {
static const bool debug = false;
// A first base case is when this clique or its parent is the root,
// in which case we return an empty Bayes net.
shared_ptr parent(parent_.lock());
if (R.get()==this || parent==R) {
BayesNet<CONDITIONAL> empty;
return empty;
}
// The root conditional
FactorGraph<FactorType> p_R(BayesNet<CONDITIONAL>(R->conditional()));
// The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form
FactorGraph<FactorType> p_Fp_Sp(BayesNet<CONDITIONAL>(parent->conditional()));
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
FactorGraph<FactorType> p_Cp_R;
p_Cp_R.push_back(p_R);
p_Cp_R.push_back(p_Fp_Sp);
p_Cp_R.push_back(p_Sp_R);
// Eliminate into a Bayes net with ordering designed to integrate out
// any variables not in *our* separator. Variables to integrate out must be
// eliminated first hence the desired ordering is [Cp\S S].
// However, an added wrinkle is that Cp might overlap with the root.
// Keys corresponding to the root should not be added to the ordering at all.
if(debug) {
p_R.print("p_R: ");
p_Fp_Sp.print("p_Fp_Sp: ");
p_Sp_R.print("p_Sp_R: ");
}
// We want to factor into a conditional of the clique variables given the
// root and the marginal on the root, integrating out all other variables.
// The integrands include any parents of this clique and the variables of
// the parent clique.
FastSet<Index> variablesAtBack;
FastSet<Index> separator;
size_t uniqueRootVariables = 0;
BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
variablesAtBack.insert(separatorIndex);
separator.insert(separatorIndex);
if(debug) cout << "At back (this): " << separatorIndex << endl;
}
BOOST_FOREACH(const Index key, R->conditional()->keys()) {
if(variablesAtBack.insert(key).second)
++ uniqueRootVariables;
if(debug) cout << "At back (root): " << key << endl;
}
Permutation toBack = Permutation::PushToBack(
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
R->conditional()->lastFrontalKey() + 1);
Permutation::shared_ptr toBackInverse(toBack.inverse());
BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
factor->permuteWithInverse(*toBackInverse); }
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<
FactorType>::Create(p_Cp_R)->eliminate(function));
// Take only the conditionals for p(S|R). We check for each variable being
// in the separator set because if some separator variables overlap with
// root variables, we cannot rely on the number of root variables, and also
// want to include those variables in the conditional.
BayesNet<CONDITIONAL> p_S_R;
BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) {
assert(conditional->nrFrontals() == 1);
if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
if(debug)
conditional->print("Taking C|R conditional: ");
p_S_R.push_front(conditional);
}
if(p_S_R.size() == separator.size())
break;
}
// Undo the permutation
if(debug) toBack.print("toBack: ");
p_S_R.permuteWithInverse(toBack);
// return the parent shortcut P(Sp|R)
assertInvariants();
return p_S_R;
}
/* ************************************************************************* */
// P(C) = \int_R P(F|S) P(S|R) P(R)
// TODO: Maybe we should integrate given parent marginal P(Cp),
// \int(Cp\S) P(F|S)P(S|Cp)P(Cp)
// Because the root clique could be very big.
/* ************************************************************************* */
template<class CONDITIONAL>
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
BayesNet<CONDITIONAL> bn(R->conditional());
if (R.get()==this) return bn;
// Combine P(F|S), P(S|R), and P(R)
BayesNet<CONDITIONAL> p_FSR = this->shortcut(R, function);
p_FSR.push_front(this->conditional());
p_FSR.push_back(R->conditional());
assertInvariants();
GenericSequentialSolver<FactorType> solver(p_FSR);
return *solver.jointFactorGraph(conditional_->keys(), function);
}
/* ************************************************************************* */
// 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> BayesTreeClique<CONDITIONAL>::joint(
shared_ptr C2, shared_ptr R, Eliminate function) {
// For now, assume neither is the root
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
FactorGraph<FactorType> joint;
if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1)
if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R)
if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R)
joint.push_back(R->conditional()->toFactor()); // P(R)
// Find the keys of both C1 and C2
vector<Index> keys1(conditional_->keys());
vector<Index> keys2(C2->conditional_->keys());
FastSet<Index> keys12;
keys12.insert(keys1.begin(), keys1.end());
keys12.insert(keys2.begin(), keys2.end());
// Calculate the marginal
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
assertInvariants();
GenericSequentialSolver<FactorType> solver(joint);
return *solver.jointFactorGraph(keys12vector, function);
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s) const {

132
gtsam/inference/BayesTree.h
View File

@ -24,11 +24,13 @@
#include <deque>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
@ -196,6 +198,8 @@ namespace gtsam {
/** return root clique */
const sharedClique& root() const { return root_; }
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** find the clique to which key belongs */
@ -207,24 +211,20 @@ namespace gtsam {
CliqueData getCliqueData() const;
/** return marginal on any variable */
typename FactorType::shared_ptr marginalFactor(Index key,
Eliminate function) const;
typename FactorType::shared_ptr marginalFactor(Index key, Eliminate function) const;
/**
* return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function
*/
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key,
Eliminate function) const;
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key, Eliminate function) const;
/** return joint on two variables */
typename FactorGraph<FactorType>::shared_ptr joint(Index key1, Index key2,
Eliminate function) const;
typename FactorGraph<FactorType>::shared_ptr joint(Index key1, Index key2, Eliminate function) const;
/** return joint on two variables as a BayesNet */
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1,
Index key2, Eliminate function) const;
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1, Index key2, Eliminate function) const;
/**
* Read only with side effects
@ -310,120 +310,24 @@ namespace gtsam {
* extra data along with the clique.
*/
template<class CONDITIONAL>
struct BayesTreeClique {
};
/* ************************************************************************* */
/**
* This is the base class for BayesTree cliques. The default and standard
* derived type is BayesTreeClique, but some algorithms, like iSAM2, use a
* different clique type in order to store extra data along with the clique.
*/
template<class DERIVED>
struct BayesTreeCliqueBase {
protected:
void assertInvariants() const;
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL> > {
public:
typedef BayesTreeClique<DERIVED> This;
typedef DERIVED DerivedType;
typedef typename DERIVED::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename boost::shared_ptr<This> shared_ptr;
typedef typename boost::weak_ptr<This> weak_ptr;
typedef typename ConditionalType::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
sharedConditional conditional_;
weak_ptr parent_;
std::list<shared_ptr> children_;
friend class BayesTree<ConditionalType>;
/** Default constructor */
BayesTreeCliqueBase() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBase(const sharedConditional& conditional);
virtual ~BayesTreeCliqueBase() {}
/** 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 ConditionalType* operator->() const { return conditional_.get(); }
/** The arrow operator accesses the conditional */
ConditionalType* 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<ConditionalType> 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);
}
typedef CONDITIONAL ConditionalType;
typedef BayesTreeClique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This> Base;
typedef boost::shared_ptr<This> shared_ptr;
BayesTreeClique() {}
BayesTreeClique(const sharedConditional& conditional) : Base(conditional) {}
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_BASE_OBJECT_NVP(Base);
}
};
}; // \struct Clique
#include <gtsam/inference/BayesTree-inl.h>
} /// namespace gtsam

251
gtsam/inference/BayesTreeCliqueBase-inl.h Normal file
View File

@ -0,0 +1,251 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueBase
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/
#pragma once
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::assertInvariants() const {
#ifndef NDEBUG
// We rely on the keys being sorted
// FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end());
// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end());
// assert(sortedUniqueKeys.size() == conditional_->size() &&
// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin()));
#endif
}
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTreeClique<CONDITIONAL>::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) {
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::print(const string& s) const {
conditional_->print(s);
}
/* ************************************************************************* */
template<class CONDITIONAL>
size_t BayesTreeClique<CONDITIONAL>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const shared_ptr& child, children_)
size += child->treeSize();
return size;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::printTree(const string& indent) const {
print(indent);
BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent+" ");
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
}
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesTreeClique<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); }
BOOST_FOREACH(const shared_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
}
#endif
if(changed) {
BOOST_FOREACH(const shared_ptr& child, children_) {
(void)child->permuteSeparatorWithInverse(inversePermutation);
}
}
assertInvariants();
return changed;
}
/* ************************************************************************* */
// The shortcut density is a conditional P(S|R) of the separator of this
// clique on the root. We can compute it recursively from the parent shortcut
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
/* ************************************************************************* */
template<class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTreeClique<CONDITIONAL>::shortcut(shared_ptr R,
Eliminate function) {
static const bool debug = false;
// A first base case is when this clique or its parent is the root,
// in which case we return an empty Bayes net.
shared_ptr parent(parent_.lock());
if (R.get()==this || parent==R) {
BayesNet<CONDITIONAL> empty;
return empty;
}
// The root conditional
FactorGraph<FactorType> p_R(BayesNet<CONDITIONAL>(R->conditional()));
// The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form
FactorGraph<FactorType> p_Fp_Sp(BayesNet<CONDITIONAL>(parent->conditional()));
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
FactorGraph<FactorType> p_Cp_R;
p_Cp_R.push_back(p_R);
p_Cp_R.push_back(p_Fp_Sp);
p_Cp_R.push_back(p_Sp_R);
// Eliminate into a Bayes net with ordering designed to integrate out
// any variables not in *our* separator. Variables to integrate out must be
// eliminated first hence the desired ordering is [Cp\S S].
// However, an added wrinkle is that Cp might overlap with the root.
// Keys corresponding to the root should not be added to the ordering at all.
if(debug) {
p_R.print("p_R: ");
p_Fp_Sp.print("p_Fp_Sp: ");
p_Sp_R.print("p_Sp_R: ");
}
// We want to factor into a conditional of the clique variables given the
// root and the marginal on the root, integrating out all other variables.
// The integrands include any parents of this clique and the variables of
// the parent clique.
FastSet<Index> variablesAtBack;
FastSet<Index> separator;
size_t uniqueRootVariables = 0;
BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
variablesAtBack.insert(separatorIndex);
separator.insert(separatorIndex);
if(debug) cout << "At back (this): " << separatorIndex << endl;
}
BOOST_FOREACH(const Index key, R->conditional()->keys()) {
if(variablesAtBack.insert(key).second)
++ uniqueRootVariables;
if(debug) cout << "At back (root): " << key << endl;
}
Permutation toBack = Permutation::PushToBack(
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
R->conditional()->lastFrontalKey() + 1);
Permutation::shared_ptr toBackInverse(toBack.inverse());
BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
factor->permuteWithInverse(*toBackInverse); }
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<
FactorType>::Create(p_Cp_R)->eliminate(function));
// Take only the conditionals for p(S|R). We check for each variable being
// in the separator set because if some separator variables overlap with
// root variables, we cannot rely on the number of root variables, and also
// want to include those variables in the conditional.
BayesNet<CONDITIONAL> p_S_R;
BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) {
assert(conditional->nrFrontals() == 1);
if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
if(debug)
conditional->print("Taking C|R conditional: ");
p_S_R.push_front(conditional);
}
if(p_S_R.size() == separator.size())
break;
}
// Undo the permutation
if(debug) toBack.print("toBack: ");
p_S_R.permuteWithInverse(toBack);
// return the parent shortcut P(Sp|R)
assertInvariants();
return p_S_R;
}
/* ************************************************************************* */
// P(C) = \int_R P(F|S) P(S|R) P(R)
// TODO: Maybe we should integrate given parent marginal P(Cp),
// \int(Cp\S) P(F|S)P(S|Cp)P(Cp)
// Because the root clique could be very big.
/* ************************************************************************* */
template<class CONDITIONAL>
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
BayesNet<CONDITIONAL> bn(R->conditional());
if (R.get()==this) return bn;
// Combine P(F|S), P(S|R), and P(R)
BayesNet<CONDITIONAL> p_FSR = this->shortcut(R, function);
p_FSR.push_front(this->conditional());
p_FSR.push_back(R->conditional());
assertInvariants();
GenericSequentialSolver<FactorType> solver(p_FSR);
return *solver.jointFactorGraph(conditional_->keys(), function);
}
/* ************************************************************************* */
// 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> BayesTreeClique<CONDITIONAL>::joint(
shared_ptr C2, shared_ptr R, Eliminate function) {
// For now, assume neither is the root
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
FactorGraph<FactorType> joint;
if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1)
if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R)
if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R)
joint.push_back(R->conditional()->toFactor()); // P(R)
// Find the keys of both C1 and C2
vector<Index> keys1(conditional_->keys());
vector<Index> keys2(C2->conditional_->keys());
FastSet<Index> keys12;
keys12.insert(keys1.begin(), keys1.end());
keys12.insert(keys2.begin(), keys2.end());
// Calculate the marginal
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
assertInvariants();
GenericSequentialSolver<FactorType> solver(joint);
return *solver.jointFactorGraph(keys12vector, function);
}
}

153
gtsam/inference/BayesTreeCliqueBase.h Normal file
View File

@ -0,0 +1,153 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueBase
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/base/types.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
namespace gtsam {
/**
* This is the base class for BayesTree cliques. The default and standard
* derived type is BayesTreeClique, but some algorithms, like iSAM2, use a
* different clique type in order to store extra data along with the clique.
*
* This class is templated on the derived class (i.e. the curiously recursive
* template pattern). The advantage of this over using virtual classes is
* that it avoids the need for casting to get the derived type. This is
* possible because all cliques in a BayesTree are the same type - if they
* were not then we'd need a virtual class.
*
* @tparam The derived clique type.
*/
template<class DERIVED>
struct BayesTreeCliqueBase {
protected:
void assertInvariants() const;
/** Default constructor */
BayesTreeCliqueBase() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBase(const sharedConditional& conditional);
public:
typedef BayesTreeClique<DERIVED> This;
typedef DERIVED DerivedType;
typedef typename DERIVED::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename boost::shared_ptr<This> shared_ptr;
typedef typename boost::weak_ptr<This> weak_ptr;
typedef typename boost::shared_ptr<DerivedType> derived_ptr;
typedef typename boost::weak_ptr<DerivedType> derived_weak_ptr;
typedef typename ConditionalType::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
sharedConditional conditional_;
derived_weak_ptr parent_;
std::list<derived_ptr> children_;
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */
static derived_ptr Create(const sharedConditional& conditional) { return boost::make_shared<DerivedType>(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 An elimination result, which is a pair<CONDITIONAL,FACTOR>
*/
static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
/** print this node */
void print(const std::string& s = "") const;
/** The arrow operator accesses the conditional */
const ConditionalType* operator->() const { return conditional_.get(); }
/** The arrow operator accesses the conditional */
ConditionalType* 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<derived_ptr>& children() { return children_; }
const std::list<derived_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<ConditionalType> 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
template<class DERIVED>
typename BayesTreeCliqueBase<DERIVED>::derived_ptr asDerived(const BayesTreeCliqueBase<DERIVED>& base) {
#ifndef NDEBUG
return boost::dynamic_pointer_cast<DERIVED>(base);
#else
return boost::static_pointer_cast<DERIVED>(base);
#endif
}
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>
}

8
gtsam/linear/GaussianConditional.h
View File

@ -52,8 +52,8 @@ public:
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
/** Store the conditional matrix as upper-triangular column-major */
typedef Matrix AbMatrix;
typedef VerticalBlockView<AbMatrix> rsd_type;
typedef Matrix RdMatrix;
typedef VerticalBlockView<RdMatrix> rsd_type;
typedef rsd_type::Block r_type;
typedef rsd_type::constBlock const_r_type;
@ -72,7 +72,7 @@ protected:
* Use R*permutation_ to get back the correct non-permuted order,
* for example when converting to the Jacobian
* */
AbMatrix matrix_;
RdMatrix matrix_;
rsd_type rsd_;
/** vector of standard deviations */
@ -169,7 +169,7 @@ public:
protected:
const AbMatrix& matrix() const { return matrix_; }
const RdMatrix& matrix() const { return matrix_; }
const rsd_type& rsd() const { return rsd_; }
public:

14
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -9,28 +9,28 @@
namespace gtsam {
/* ************************************************************************* */
VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) {
double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) {
// Get magnitude of each update and find out which segment Delta falls in
assert(Delta >= 0.0);
double DeltaSq = Delta*Delta;
double x_u_norm_sq = x_u.vector().squaredNorm();
double x_n_norm_sq = x_n.vector().squaredNorm();
double x_u_norm_sq = dx_u.vector().squaredNorm();
double x_n_norm_sq = dx_n.vector().squaredNorm();
if(verbose) cout << "Steepest descent magnitude " << sqrt(x_u_norm_sq) << ", Newton's method magnitude " << sqrt(x_n_norm_sq) << endl;
if(DeltaSq < x_u_norm_sq) {
// Trust region is smaller than steepest descent update
VectorValues x_d = VectorValues::SameStructure(x_u);
x_d.vector() = x_u.vector() * sqrt(DeltaSq / x_u_norm_sq);
VectorValues x_d = VectorValues::SameStructure(dx_u);
x_d.vector() = dx_u.vector() * sqrt(DeltaSq / x_u_norm_sq);
if(verbose) cout << "In steepest descent region with fraction " << sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
return x_d;
} else if(DeltaSq < x_n_norm_sq) {
// Trust region boundary is between steepest descent point and Newton's method point
return ComputeBlend(Delta, x_u, x_n);
return ComputeBlend(Delta, dx_u, dx_n);
} else {
assert(DeltaSq >= x_n_norm_sq);
if(verbose) cout << "In pure Newton's method region" << endl;
// Trust region is larger than Newton's method point
return x_n;
return dx_n;
}
}

5
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -95,10 +95,11 @@ struct DoglegOptimizerImpl {
* GaussianBayesNet, containing GaussianConditional s.
*
* @param Delta The trust region radius
* @param bayesNet The Bayes' net \f$ (R,d) \f$ as described above.
* @param dx_u The steepest descent point, i.e. the Cauchy point
* @param dx_n The Gauss-Newton point
* @return The dogleg point \f$ \delta x_d \f$
*/
static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false);
static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false);
/** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of
* the function

68
gtsam/nonlinear/ISAM2.h
View File

@ -106,71 +106,32 @@ struct ISAM2Result {
};
template<class CONDITIONAL>
struct ISAM2Clique : public BayesTreeClique<CONDITIONAL> {
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL> > {
typedef ISAM2Clique<CONDITIONAL> This;
typedef BayesTreeClique<CONDITIONAL> Base;
typedef BayesTreeCliqueBase<This> Base;
typedef boost::shared_ptr<This> shared_ptr;
typename Base::FactorType::shared_ptr cachedFactor_;
Vector gradientContribution_;
/** 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) {}
/** Access the gradient contribution */
const Vector& gradientContribution() const { return gradientContribution_; }
/** Construct from a conditional */
ISAM2Clique(const typename Base::sharedConditional& conditional) : Base(conditional) {}
ISAM2Clique(const sharedConditional& conditional) : Base(conditional) {
throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); }
/** 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;
/** Construct from an elimination result */
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
// Compute gradient contribution
const ConditionalType& conditional(*result.first);
gradient << -(conditional.get_R() * conditional.permutation().transpose()).transpose * conditional.get_d(),
-conditional.get_S() * conditional.get_d();
}
private:
@ -180,6 +141,7 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
ar & BOOST_SERIALIZATION_NVP(gradientContribution_);
}
};