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

(in branch) make check passes after refactoring and code changes for including -inl.h files from .h files

release/4.3a0
Richard Roberts 2011-12-12 23:19:31 +00:00
parent 3b139cbae2
commit 8d5facb09e
39 changed files with 246 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="6" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@ -102,7 +102,9 @@
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule>
<storageModule moduleId="refreshScope"/>
<storageModule moduleId="refreshScope" versionNumber="1">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">

2
.project
View File

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

2
gtsam/base/BTree.h
View File

@ -147,7 +147,7 @@ namespace gtsam {
/** add a key-value pair */
BTree add(const KEY& x, const VALUE& d) const {
return add(make_pair(x, d));
return add(std::make_pair(x, d));
}
/** member predicate */

2
gtsam/base/DSF.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
DSF(const std::set<KEY>& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); }
// create a new singleton, does nothing if already exists
Self makeSet(const KEY& key) const { if (mem(key)) return *this; else return this->add(key, key); }
Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); }
// find the label of the set in which {key} lives
Label findSet(const KEY& key) const {

1
gtsam/inference/BayesNet-inl.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/inference/BayesNet.h>
#include <iostream>
#include <fstream>

4
gtsam/inference/BayesNet.h
View File

@ -206,4 +206,6 @@ private:
}
}; // BayesNet
} /// namespace gtsam
} // namespace gtsam
#include <gtsam/inference/BayesNet-inl.h>

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

@ -24,7 +24,7 @@
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
#include <gtsam/inference/GenericSequentialSolver.h>
#include <iostream>
#include <algorithm>
@ -501,7 +501,7 @@ namespace gtsam {
this->removeClique(clique);
// remove path above me
this->removePath(typename BayesTreeClique<CONDITIONAL>::shared_ptr(clique->parent_.lock()), bn, orphans);
this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
// add children to list of orphans (splice also removed them from clique->children_)
orphans.splice (orphans.begin(), clique->children_);

31
gtsam/inference/BayesTree.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
public:
typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
typedef CONDITIONAL ConditionalType;
@ -178,11 +178,23 @@ namespace gtsam {
/** check equality */
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree) const {
root_->cloneToBayesTree(*newTree);
cloneTo(newTree, root());
}
private:
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree, const sharedClique& root) const {
if(root) {
sharedClique newClique = root->clone();
newTree->insert(newClique);
BOOST_FOREACH(const sharedClique& childClique, root->children()) {
cloneTo(newTree, childClique);
}
}
}
public:
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
@ -315,14 +327,16 @@ namespace gtsam {
* extra data along with the clique.
*/
template<class CONDITIONAL>
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL> > {
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
public:
typedef CONDITIONAL ConditionalType;
typedef BayesTreeClique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This> Base;
typedef BayesTreeCliqueBase<This, CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeClique() {}
BayesTreeClique(const sharedConditional& conditional) : Base(conditional) {}
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
private:
/** Serialization function */
@ -333,6 +347,7 @@ namespace gtsam {
}
};
#include <gtsam/inference/BayesTree-inl.h>
} /// namespace gtsam
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>

93
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -17,11 +17,13 @@
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::assertInvariants() const {
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::assertInvariants() const {
#ifndef NDEBUG
// We rely on the keys being sorted
// FastVector<Index> sortedUniqueKeys(conditional_->begin(), conditional_->end());
@ -33,52 +35,60 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTreeClique<CONDITIONAL>::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) {
template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBase<DERIVED,CONDITIONAL>::BayesTreeCliqueBase(const sharedConditional& conditional) :
conditional_(conditional) {
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::print(const string& s) const {
template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBase<DERIVED,CONDITIONAL>::BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
conditional_(result.first) {
assertInvariants();
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::print(const std::string& s) const {
conditional_->print(s);
}
/* ************************************************************************* */
template<class CONDITIONAL>
size_t BayesTreeClique<CONDITIONAL>::treeSize() const {
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBase<DERIVED,CONDITIONAL>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const shared_ptr& child, children_)
BOOST_FOREACH(const derived_ptr& child, children_)
size += child->treeSize();
return size;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::printTree(const string& indent) const {
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::printTree(const std::string& indent) const {
print(indent);
BOOST_FOREACH(const shared_ptr& child, children_)
BOOST_FOREACH(const derived_ptr& child, children_)
child->printTree(indent+" ");
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTreeClique<CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::permuteWithInverse(const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const shared_ptr& child, children_) {
BOOST_FOREACH(const derived_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
}
assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesTreeClique<CONDITIONAL>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBase<DERIVED,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_) {
BOOST_FOREACH(const derived_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
}
@ -97,9 +107,8 @@ namespace gtsam {
// 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) {
template<class DERIVED, class CONDITIONAL>
BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::shortcut(derived_ptr R, Eliminate function) {
static const bool debug = false;
@ -109,16 +118,16 @@ namespace gtsam {
shared_ptr parent(parent_.lock());
if (R.get()==this || parent==R) {
BayesNet<CONDITIONAL> empty;
BayesNet<ConditionalType> empty;
return empty;
}
// The root conditional
FactorGraph<FactorType> p_R(BayesNet<CONDITIONAL>(R->conditional()));
FactorGraph<FactorType> p_R(BayesNet<ConditionalType>(R->conditional()));
// The parent clique has a CONDITIONAL for each frontal node in Fp
// The parent clique has a ConditionalType 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()));
FactorGraph<FactorType> p_Fp_Sp(BayesNet<ConditionalType>(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));
@ -151,29 +160,29 @@ namespace gtsam {
BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) {
variablesAtBack.insert(separatorIndex);
separator.insert(separatorIndex);
if(debug) cout << "At back (this): " << separatorIndex << endl;
if(debug) std::cout << "At back (this): " << separatorIndex << std::endl;
}
BOOST_FOREACH(const Index key, R->conditional()->keys()) {
if(variablesAtBack.insert(key).second)
++ uniqueRootVariables;
if(debug) cout << "At back (root): " << key << endl;
if(debug) std::cout << "At back (root): " << key << std::endl;
}
Permutation toBack = Permutation::PushToBack(
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
std::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<
typename BayesNet<ConditionalType>::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) {
BayesNet<ConditionalType> p_S_R;
BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) {
assert(conditional->nrFrontals() == 1);
if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) {
if(debug)
@ -199,16 +208,16 @@ namespace gtsam {
// \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) {
template<class DERIVED, class CONDITIONAL>
FactorGraph<typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::FactorType> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::marginal(
derived_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());
BayesNet<ConditionalType> 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);
BayesNet<ConditionalType> p_FSR = this->shortcut(R, function);
p_FSR.push_front(this->conditional());
p_FSR.push_back(R->conditional());
@ -220,9 +229,9 @@ 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> BayesTreeClique<CONDITIONAL>::joint(
shared_ptr C2, shared_ptr R, Eliminate function) {
template<class DERIVED, class CONDITIONAL>
FactorGraph<typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::FactorType> BayesTreeCliqueBase<DERIVED,CONDITIONAL>::joint(
derived_ptr C2, derived_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)
@ -234,14 +243,14 @@ namespace gtsam {
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());
std::vector<Index> keys1(conditional_->keys());
std::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());
std::vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
assertInvariants();
GenericSequentialSolver<FactorType> solver(joint);

58
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -24,6 +24,8 @@
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTree; }
namespace gtsam {
/**
@ -39,9 +41,21 @@ namespace gtsam {
*
* @tparam The derived clique type.
*/
template<class DERIVED>
template<class DERIVED, class CONDITIONAL>
struct BayesTreeCliqueBase {
public:
typedef BayesTreeCliqueBase<DERIVED,CONDITIONAL> This;
typedef DERIVED DerivedType;
typedef CONDITIONAL ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
typedef typename ConditionalType::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
protected:
void assertInvariants() const;
@ -51,18 +65,10 @@ namespace gtsam {
/** 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;
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result);
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
std::list<derived_ptr> children_;
@ -77,19 +83,7 @@ namespace gtsam {
*/
static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const {
sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_));
sharedClique newClique = newTree.addClique(newConditional, parent_clique);
if (cachedFactor_)
newClique->cachedFactor_ = cachedFactor_->clone();
else newClique->cachedFactor_ = typename FactorType::shared_ptr();
if (!parent_clique) {
newTree.root_ = newClique;
}
BOOST_FOREACH(const shared_ptr& childClique, children_) {
childClique->cloneToBayesTree(newTree, newClique);
}
}
derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); }
/** print this node */
void print(const std::string& s = "") const;
@ -128,19 +122,21 @@ namespace gtsam {
/** 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);
BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function);
/** return the marginal P(C) of the clique */
FactorGraph<FactorType> marginal(shared_ptr root, Eliminate function);
FactorGraph<FactorType> marginal(derived_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);
FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function);
bool equals(const This& other, double tol=1e-9) const {
return (!conditional_ && !other.conditional()) ||
conditional_->equals(*(other.conditional()), tol);
}
friend class BayesTree<ConditionalType, DerivedType>;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -153,8 +149,8 @@ namespace gtsam {
}; // \struct Clique
template<class DERIVED>
typename BayesTreeCliqueBase<DERIVED>::derived_ptr asDerived(const BayesTreeCliqueBase<DERIVED>& base) {
template<class DERIVED, class CONDITIONAL>
typename BayesTreeCliqueBase<DERIVED,CONDITIONAL>::derived_ptr asDerived(const BayesTreeCliqueBase<DERIVED,CONDITIONAL>& base) {
#ifndef NDEBUG
return boost::dynamic_pointer_cast<DERIVED>(base);
#else
@ -162,6 +158,4 @@ namespace gtsam {
#endif
}
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>
}

1
gtsam/inference/EliminationTree-inl.h
View File

@ -9,7 +9,6 @@
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/IndexConditional.h>

4
gtsam/inference/EliminationTree.h
View File

@ -10,10 +10,10 @@
#include <gtsam/base/FastSet.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
namespace gtsam {
@ -140,3 +140,5 @@ struct DisconnectedGraphException : public std::exception {
};
}
#include <gtsam/inference/EliminationTree-inl.h>

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

@ -24,7 +24,7 @@
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/DSF.h>
#include <boost/foreach.hpp>

1
gtsam/inference/FactorGraph.h
View File

@ -239,3 +239,4 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
} // namespace gtsam
#include <gtsam/inference/FactorGraph-inl.h>

21
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -18,10 +18,10 @@
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>
@ -77,9 +77,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
GenericSequentialSolver<FACTOR>::eliminate(
typename EliminationTree<FACTOR>::Eliminate function) const {
typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > //
GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
return eliminationTree_->eliminate(function);
}
@ -87,8 +86,7 @@ namespace gtsam {
template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointFactorGraph(
const std::vector<Index>& js,
typename EliminationTree<FACTOR>::Eliminate function) const {
const std::vector<Index>& js, Eliminate function) const {
// Compute a COLAMD permutation with the marginal variable constrained to the end.
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
@ -127,10 +125,9 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename FACTOR::shared_ptr //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
typename EliminationTree<FACTOR>::Eliminate function) const {
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
// Create a container for the one variable index
vector<Index> js(1);
std::vector<Index> js(1);
js[0] = j;
// Call joint and return the only factor in the factor graph it returns

36
gtsam/inference/GenericSequentialSolver.h
View File

@ -19,9 +19,15 @@
#pragma once
#include <utility>
#include <boost/function.hpp>
#include <vector>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/EliminationTree.h>
namespace gtsam { class VariableIndex; }
namespace gtsam { template<class FACTOR> class EliminationTree; }
namespace gtsam { template<class FACTOR> class FactorGraph; }
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
namespace gtsam {
@ -43,7 +49,12 @@ namespace gtsam {
protected:
typedef typename FactorGraph<FACTOR>::shared_ptr sharedFactorGraph;
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
typedef std::pair<
boost::shared_ptr<typename FACTOR::ConditionalType>,
boost::shared_ptr<FACTOR> > EliminationResult;
typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
/** Store the original factors for computing marginals
* TODO Frank says: really? Marginals should be computed from result.
@ -51,15 +62,14 @@ namespace gtsam {
sharedFactorGraph factors_;
/** Store column structure of the factor graph. Why? */
VariableIndex::shared_ptr structure_;
boost::shared_ptr<VariableIndex> structure_;
/** Elimination tree that performs elimination */
typedef EliminationTree<FACTOR> EliminationTree_;
typename EliminationTree<FACTOR>::shared_ptr eliminationTree_;
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
/** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree_)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
public:
@ -76,7 +86,7 @@ namespace gtsam {
*/
GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const VariableIndex::shared_ptr& variableIndex);
const boost::shared_ptr<VariableIndex>& variableIndex);
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
@ -95,25 +105,23 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr
eliminate(typename EliminationTree<FACTOR>::Eliminate function) const;
typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > eliminate(Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph.
*/
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(
const std::vector<Index>& js,
typename EliminationTree<FACTOR>::Eliminate function) const;
const std::vector<Index>& js, Eliminate function) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
typename FACTOR::shared_ptr marginalFactor(Index j,
typename EliminationTree<FACTOR>::Eliminate function) const;
typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const;
}; // GenericSequentialSolver
} // namespace gtsam
#include <gtsam/inference/GenericSequentialSolver-inl.h>

2
gtsam/inference/ISAM-inl.h
View File

@ -41,7 +41,7 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree
BayesNet<CONDITIONAL> bn;
removeTop(newFactors.keys(), bn, orphans);
this->removeTop(newFactors.keys(), bn, orphans);
FG factors(bn);
// add the factors themselves

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

@ -21,11 +21,9 @@
#include <gtsam/base/timing.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/ClusterTree-inl.h>
#include <gtsam/inference/EliminationTree.h>
#include <boost/foreach.hpp>
#include <boost/lambda/bind.hpp>

2
gtsam/inference/JunctionTree.h
View File

@ -110,3 +110,5 @@ namespace gtsam {
}; // JunctionTree
} // namespace gtsam
#include <gtsam/inference/JunctionTree-inl.h>

11
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -18,19 +18,14 @@
#include <boost/make_shared.hpp>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/IndexConditional.h>
namespace gtsam {
using namespace std;
// Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<IndexFactor>;
template class BayesNet<IndexConditional>;
template class EliminationTree<IndexFactor>;
/* ************************************************************************* */
SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet) :
FactorGraph<IndexFactor>(bayesNet) {}

11
gtsam/inference/SymbolicFactorGraph.h
View File

@ -17,8 +17,13 @@
#pragma once
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/IndexFactor.h>
namespace gtsam { template<class FACTOR> class EliminationTree; }
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
namespace gtsam { class IndexConditional; }
namespace gtsam {
@ -72,7 +77,7 @@ namespace gtsam {
* Combine and eliminate can also be called separately, but for this and
* derived classes calling them separately generally does extra work.
*/
std::pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr>
std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
/* Template function implementation */

2
gtsam/inference/SymbolicMultifrontalSolver.cpp
View File

@ -16,7 +16,7 @@
*/
#include <gtsam/inference/SymbolicMultifrontalSolver.h>
#include <gtsam/inference/JunctionTree-inl.h>
#include <gtsam/inference/JunctionTree.h>
namespace gtsam {

3
gtsam/inference/inference.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
@ -77,4 +76,4 @@ namespace gtsam {
return PermutationCOLAMD_(variableIndex, cmember);
}
} /// namespace gtsam
} // namespace gtsam

2
gtsam/inference/tests/testClusterTree.cpp
View File

@ -27,7 +27,7 @@ using namespace boost::assign;
using namespace gtsam;
// explicit instantiation and typedef
template class ClusterTree<SymbolicFactorGraph>;
namespace gtsam { template class ClusterTree<SymbolicFactorGraph>; }
typedef ClusterTree<SymbolicFactorGraph> SymbolicClusterTree;
/* ************************************************************************* */

2
gtsam/inference/tests/testJunctionTree.cpp
View File

@ -17,6 +17,7 @@
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
@ -35,6 +36,7 @@ using namespace boost::assign;
#include <gtsam/inference/SymbolicSequentialSolver.h>
using namespace gtsam;
using namespace std;
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
typedef BayesTree<IndexConditional> SymbolicBayesTree;

2
gtsam/linear/GaussianConditional.h
View File

@ -24,7 +24,6 @@
#include <gtsam/base/types.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
// Forward declaration to friend unit tests
@ -39,6 +38,7 @@ namespace gtsam {
// Forward declarations
class GaussianFactor;
class JacobianFactor;
/**
* A conditional Gaussian functions as the node in a Bayes network

2
gtsam/linear/KalmanFilter.h
View File

@ -22,6 +22,8 @@
#include <gtsam/linear/GaussianConditional.h>
namespace gtsam { class SharedDiagonal; }
namespace gtsam {
class KalmanFilter {

1
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>

2
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -18,6 +18,8 @@
*/
#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;

21
gtsam/nonlinear/GaussianISAM2.cpp → gtsam/nonlinear/GaussianISAM2-inl.h
View File

@ -14,7 +14,8 @@ using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, double threshold,
template<class CLIQUE>
void optimize2(const boost::shared_ptr<CLIQUE>& clique, double threshold,
vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
@ -84,7 +85,7 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, doubl
}
// Recurse to children
BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_) {
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
optimize2(child, threshold, changed, replaced, delta, count);
}
}
@ -92,14 +93,15 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, doubl
/* ************************************************************************* */
// fast full version without threshold
void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& delta) {
template<class CLIQUE>
void optimize2(const boost::shared_ptr<CLIQUE>& clique, VectorValues& delta) {
// parents are assumed to already be solved and available in result
(*clique)->rhs(delta);
(*clique)->solveInPlace(delta);
// Solve chilren recursively
BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_) {
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
optimize2(child, delta);
}
}
@ -114,7 +116,8 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, Vecto
//}
/* ************************************************************************* */
int optimize2(const BayesTree<GaussianConditional>::sharedClique& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
template<class CLIQUE>
int optimize2(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
vector<bool> changed(keys.size(), false);
int count = 0;
// starting from the root, call optimize on each conditional
@ -123,18 +126,20 @@ int optimize2(const BayesTree<GaussianConditional>::sharedClique& root, double t
}
/* ************************************************************************* */
void nnz_internal(const BayesTree<GaussianConditional>::sharedClique& clique, int& result) {
template<class CLIQUE>
void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
int dimR = (*clique)->dim();
int dimSep = (*clique)->get_S().cols() - dimR;
result += ((dimR+1)*dimR)/2 + dimSep*dimR;
// traverse the children
BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_) {
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
nnz_internal(child, result);
}
}
/* ************************************************************************* */
int calculate_nnz(const BayesTree<GaussianConditional>::sharedClique& clique) {
template<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique) {
int result = 0;
// starting from the root, add up entries of frontal and conditional matrices of each conditional
nnz_internal(clique, result);

11
gtsam/nonlinear/GaussianISAM2.h
View File

@ -42,7 +42,8 @@ public:
};
// optimize the BayesTree, starting from the root
void optimize2(const BayesTree<GaussianConditional>::sharedClique& root, VectorValues& delta);
template<class CLIQUE>
void optimize2(const boost::shared_ptr<CLIQUE>& root, VectorValues& delta);
// optimize the BayesTree, starting from the root; "replaced" needs to contain
// all variables that are contained in the top of the Bayes tree that has been
@ -52,10 +53,14 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& root, VectorV
// and recursive backsubstitution might eventually stop if none of the changed
// variables are contained in the subtree.
// returns the number of variables that were solved for
int optimize2(const BayesTree<GaussianConditional>::sharedClique& root,
template<class CLIQUE>
int optimize2(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
int calculate_nnz(const BayesTree<GaussianConditional>::sharedClique& clique);
template<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
}/// namespace gtsam
#include <gtsam/nonlinear/GaussianISAM2-inl.h>

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

@ -81,7 +81,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
*
* Alternatively could we trace up towards the root for each variable here?
*/
static void FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
static void FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
/**
* Apply expmap to the given values, but only for indices appearing in
@ -148,7 +148,7 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
const size_t originalDim = delta->dim();
const size_t originalnVars = delta->size();
delta.container().append(dims);
delta.container().vector().segment(originalDim, newDim) = Vector::Zero(newDim);
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
delta.permutation().resize(originalnVars + newTheta.size());
{
_VariableAdder vadder(ordering, delta, originalnVars);
@ -188,7 +188,7 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(Permu
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<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;
@ -202,7 +202,7 @@ void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIO
if(debug) clique->print("Key(s) marked in clique ");
if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
}
BOOST_FOREACH(const typename BayesTreeClique<CONDITIONAL>::shared_ptr& child, clique->children_) {
BOOST_FOREACH(const typename ISAM2Clique<CONDITIONAL>::shared_ptr& child, clique->children_) {
FindAll(child, keys, markedMask);
}
}

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

@ -286,7 +286,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
toc(5,"eliminate");
tic(6,"insert");
BayesTree<CONDITIONAL>::clear();
this->clear();
this->insert(newRoot);
toc(6,"insert");
@ -310,6 +310,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
tic(1,"relinearizeAffected");
GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys));
if(debug) factors.print("Relinearized factors: ");
toc(1,"relinearizeAffected");
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }

44
gtsam/nonlinear/ISAM2.h
View File

@ -106,21 +106,18 @@ struct ISAM2Result {
};
template<class CONDITIONAL>
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL> > {
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDITIONAL> {
typedef ISAM2Clique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This> Base;
typedef BayesTreeCliqueBase<This,CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef CONDITIONAL ConditionalType;
typedef typename ConditionalType::shared_ptr sharedConditional;
typename Base::FactorType::shared_ptr cachedFactor_;
Vector gradientContribution_;
/** Access the cached factor */
typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** Access the gradient contribution */
const Vector& gradientContribution() const { return gradientContribution_; }
/** Construct from a conditional */
ISAM2Clique(const sharedConditional& conditional) : Base(conditional) {
throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); }
@ -130,8 +127,33 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL> > {
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();
gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(),
-conditional.get_S().transpose() * conditional.get_d();
}
/** Produce a deep copy, copying the cached factor and gradient contribution */
shared_ptr clone() const {
shared_ptr copy(new ISAM2Clique(make_pair(
sharedConditional(new ConditionalType(*Base::conditional_)),
cachedFactor_ ? cachedFactor_->clone() : typename Base::FactorType::shared_ptr())));
copy->gradientContribution_ = gradientContribution_;
return copy;
}
/** Access the cached factor */
typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** Access the gradient contribution */
const Vector& gradientContribution() const { return gradientContribution_; }
void permuteWithInverse(const Permutation& inversePermutation) {
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
Base::permuteWithInverse(inversePermutation);
}
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
return Base::permuteSeparatorWithInverse(inversePermutation);
}
private:
@ -214,7 +236,7 @@ public:
typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class
void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
void cloneTo(boost::shared_ptr<This>& newISAM2) const {
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
Base::cloneTo(bayesTree);
newISAM2->theta_ = theta_;

2
gtsam/nonlinear/Makefile.am
View File

@ -30,7 +30,7 @@ headers += DoglegOptimizer.h DoglegOptimizer-inl.h
# Nonlinear iSAM(2)
headers += NonlinearISAM.h NonlinearISAM-inl.h
headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h
sources += GaussianISAM2.cpp
headers += GaussianISAM2.h GaussianISAM2-inl.h
# Nonlinear constraints
headers += NonlinearEquality.h

4
gtsam/nonlinear/NonlinearFactor.h
View File

@ -249,7 +249,7 @@ public:
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const VALUES& c) const {
if (active(c))
if (this->active(c))
return 0.5 * noiseModel_->distance(unwhitenedError(c));
else
return 0.0;
@ -262,7 +262,7 @@ public:
*/
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
// Only linearize if the factor is active
if (!active(x))
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
// Create the set of terms - Jacobians for each index

1
gtsam/nonlinear/Ordering.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <map>
#include <set>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/inference.h>

1
gtsam/nonlinear/tests/testOrdering.cpp
View File

@ -15,6 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/nonlinear/Ordering.h>
using namespace gtsam;

9
tests/testGaussianISAM2.cpp
View File

@ -167,7 +167,8 @@ TEST(ISAM2, optimize2) {
conditional->solveInPlace(expected);
// Clique
GaussianISAM2<planarSLAM::Values>::sharedClique clique(GaussianISAM2<planarSLAM::Values>::Clique::Create(conditional));
GaussianISAM2<planarSLAM::Values>::sharedClique clique(
GaussianISAM2<planarSLAM::Values>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
VectorValues actual(theta.dims(ordering));
conditional->rhs(actual);
optimize2(clique, actual);
@ -192,9 +193,13 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu
}
/* ************************************************************************* */
TEST(ISAM2, slamlike_solution)
TEST_UNSAFE(ISAM2, slamlike_solution)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;