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

Major change, elimination functions are now passed an "Eliminate function", so you can select at run-time which factorization method is used (symbolic, QR, etc...).

release/4.3a0
Frank Dellaert 2011-03-24 19:27:12 +00:00
parent ba8cfe2554
commit 5c193422af
56 changed files with 2078 additions and 1832 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

176
.cproject
View File

@ -676,6 +676,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -692,6 +700,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -756,6 +780,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testSymbolicFactorGraph.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testSymbolicBayesNet.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -820,14 +860,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -876,6 +908,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1411,6 +1451,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVirtual.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testVirtual.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/timeVirtual.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timeVirtual.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1443,6 +1499,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2359,6 +2431,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2375,6 +2455,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2439,6 +2535,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testSymbolicFactorGraph.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testSymbolicBayesNet.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2503,14 +2615,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2559,6 +2663,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3094,6 +3206,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVirtual.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testVirtual.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/timeVirtual.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timeVirtual.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3126,6 +3254,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

1
gtsam/base/Testable.h
View File

@ -97,6 +97,7 @@ namespace gtsam {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
if (!actual || !expected) return false;
return (actual->equals(*expected, tol_));
}
};

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

@ -280,8 +280,8 @@ namespace gtsam {
// TODO, why do we actually return a shared pointer, why does eliminate?
/* ************************************************************************* */
template<class CONDITIONAL>
BayesNet<CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R) {
BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R,
Eliminate function) {
static const bool debug = false;
@ -296,17 +296,17 @@ namespace gtsam {
}
// The root conditional
FactorGraph<typename CONDITIONAL::FactorType> p_R(*R);
FactorGraph<FactorType> p_R(*R);
// The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form
FactorGraph<typename CONDITIONAL::FactorType> p_Fp_Sp(*parent);
FactorGraph<FactorType> p_Fp_Sp(*parent);
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
FactorGraph<typename CONDITIONAL::FactorType> p_Sp_R(parent->shortcut(R));
FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function));
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
FactorGraph<typename CONDITIONAL::FactorType> p_Cp_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);
@ -345,9 +345,10 @@ namespace gtsam {
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
R->back()->key() + 1);
Permutation::shared_ptr toBackInverse(toBack.inverse());
BOOST_FOREACH(const typename CONDITIONAL::FactorType::shared_ptr& factor, p_Cp_R) {
BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) {
factor->permuteWithInverse(*toBackInverse); }
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<typename CONDITIONAL::FactorType>::Create(p_Cp_R)->eliminate());
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
@ -380,34 +381,37 @@ namespace gtsam {
// Because the root clique could be very big.
/* ************************************************************************* */
template<class CONDITIONAL>
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::marginal(
shared_ptr R, Eliminate function) {
// If we are the root, just return this root
// NOTE: immediately cast to a factor graph
if (R.get()==this) return *R;
// Combine P(F|S), P(S|R), and P(R)
BayesNet<CONDITIONAL> p_FSR = this->shortcut(R);
BayesNet<CONDITIONAL> p_FSR = this->shortcut(R, function);
p_FSR.push_front(*this);
p_FSR.push_back(*R);
assertInvariants();
return *GenericSequentialSolver<typename CONDITIONAL::FactorType>(p_FSR).jointFactorGraph(keys());
GenericSequentialSolver<FactorType> solver(p_FSR);
return *solver.jointFactorGraph(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> BayesTree<CONDITIONAL>::Clique::joint(shared_ptr C2, shared_ptr R) {
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::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<typename CONDITIONAL::FactorType> joint;
if (!isRoot()) joint.push_back(*this); // P(F1|S1)
if (!isRoot()) joint.push_back(shortcut(R)); // P(S1|R)
if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->shortcut(R)); // P(S2|R)
joint.push_back(*R); // P(R)
FactorGraph<FactorType> joint;
if (!isRoot()) joint.push_back(*this); // P(F1|S1)
if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R)
if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R)
joint.push_back(*R); // P(R)
// Find the keys of both C1 and C2
vector<Index> keys1(keys());
@ -420,7 +424,8 @@ namespace gtsam {
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
assertInvariants();
return *GenericSequentialSolver<typename CONDITIONAL::FactorType>(joint).jointFactorGraph(keys12vector);
GenericSequentialSolver<FactorType> solver(joint);
return *solver.jointFactorGraph(keys12vector, function);
}
/* ************************************************************************* */
@ -729,53 +734,59 @@ namespace gtsam {
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CONDITIONAL>
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL>::marginalFactor(Index key) const {
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL>::marginalFactor(
Index key, Eliminate function) const {
// get clique containing key
sharedClique clique = (*this)[key];
// calculate or retrieve its marginal
FactorGraph<typename CONDITIONAL::FactorType> cliqueMarginal = clique->marginal(root_);
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(cliqueMarginal).marginalFactor(key);
return GenericSequentialSolver<FactorType> (cliqueMarginal).marginalFactor(
key, function);
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(
Index key, Eliminate function) const {
// calculate marginal as a factor graph
FactorGraph<typename CONDITIONAL::FactorType> fg;
fg.push_back(this->marginalFactor(key));
FactorGraph<FactorType> fg;
fg.push_back(this->marginalFactor(key,function));
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(fg).eliminate();
return GenericSequentialSolver<FactorType>(fg).eliminate(function);
}
/* ************************************************************************* */
// Find two cliques, their joint, then marginalizes
/* ************************************************************************* */
template<class CONDITIONAL>
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
BayesTree<CONDITIONAL>::joint(Index key1, Index key2) const {
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr BayesTree<
CONDITIONAL>::joint(Index key1, Index key2, Eliminate function) const {
// get clique C1 and C2
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
// calculate joint
FactorGraph<typename CONDITIONAL::FactorType> p_C1C2(C1->joint(C2, root_));
FactorGraph<FactorType> p_C1C2(C1->joint(C2, root_, function));
// eliminate remaining factor graph to get requested joint
vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(p_C1C2).jointFactorGraph(key12);
GenericSequentialSolver<FactorType> solver(p_C1C2);
return solver.jointFactorGraph(key12,function);
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet(Index key1, Index key2) const {
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet(
Index key1, Index key2, Eliminate function) const {
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(*this->joint(key1, key2)).eliminate();
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<FactorType> (
*this->joint(key1, key2, function)).eliminate(function);
}
/* ************************************************************************* */

24
gtsam/inference/BayesTree.h
View File

@ -46,6 +46,8 @@ namespace gtsam {
typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
/**
* A Clique in the tree is an incomplete Bayes net: the variables
@ -63,7 +65,7 @@ namespace gtsam {
weak_ptr parent_;
std::list<shared_ptr> children_;
std::list<Index> separator_; /** separator keys */
typename CONDITIONAL::FactorType::shared_ptr cachedFactor_;
typename FactorType::shared_ptr cachedFactor_;
friend class BayesTree<CONDITIONAL>;
@ -96,7 +98,7 @@ namespace gtsam {
size_t treeSize() const;
/** Access the cached factor (this is a hack) */
typename CONDITIONAL::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
typename FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** print this node and entire subtree below it */
void printTree(const std::string& indent="") const;
@ -113,13 +115,13 @@ namespace gtsam {
/** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version
BayesNet<CONDITIONAL> shortcut(shared_ptr root);
BayesNet<CONDITIONAL> shortcut(shared_ptr root, Eliminate function);
/** return the marginal P(C) of the clique */
FactorGraph<typename CONDITIONAL::FactorType> marginal(shared_ptr root);
FactorGraph<FactorType> marginal(shared_ptr root, Eliminate function);
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<typename CONDITIONAL::FactorType> joint(shared_ptr C2, shared_ptr root);
FactorGraph<FactorType> joint(shared_ptr C2, shared_ptr root, Eliminate function);
}; // \struct Clique
@ -262,20 +264,24 @@ namespace gtsam {
CliqueData getCliqueData() const;
/** return marginal on any variable */
typename CONDITIONAL::FactorType::shared_ptr marginalFactor(Index key) const;
typename CONDITIONAL::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) const;
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key,
Eliminate function) const;
/** return joint on two variables */
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr joint(Index key1, Index key2) 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) const;
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1,
Index key2, Eliminate function) const;
/**
* Read only with side effects

49
gtsam/inference/Conditional.h
View File

@ -24,10 +24,8 @@
#include <boost/foreach.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Permutation.h>
namespace gtsam {
@ -168,18 +166,6 @@ public:
/** print */
void print(const std::string& s = "Conditional") const;
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/**
* Permutes the Conditional, but for efficiency requires the permutation
* to already be inverted.
*/
void permuteWithInverse(const Permutation& inversePermutation);
private:
/** Serialization function */
friend class boost::serialization::access;
@ -200,37 +186,4 @@ void Conditional<KEY>::print(const std::string& s) const {
std::cout << ")" << std::endl;
}
/* ************************************************************************* */
template<typename KEY>
bool Conditional<KEY>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
}
}
assertInvariants();
return parentChanged;
}
/* ************************************************************************* */
template<typename KEY>
void Conditional<KEY>::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
#endif
FactorType::permuteWithInverse(inversePermutation);
assertInvariants();
}
}
} // gtsam

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

@ -27,8 +27,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTree<FACTOR>::sharedFactor
EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminate_(
Eliminate function, Conditionals& conditionals) const {
static const bool debug = false;
@ -43,11 +43,11 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
factors.push_back(child->eliminate_(conditionals)); }
factors.push_back(child->eliminate_(function, conditionals)); }
// Combine all factors (from this node and from subtrees) into a joint factor
pair<typename BayesNet::shared_ptr, typename FACTOR::shared_ptr> eliminated(
FACTOR::CombineAndEliminate(factors, 1));
pair<typename BayesNet::shared_ptr, typename FACTOR::shared_ptr>
eliminated(function(factors, 1));
conditionals[this->key_] = eliminated.first->front();
if(debug) cout << "Eliminated " << this->key_ << " to get:\n";
@ -57,39 +57,6 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
return eliminated.second;
}
/* ************************************************************************* */
// This is the explicit specialization for symbolic factors, i.e. IndexFactor
template<> inline FastSet<Index> EliminationTree<IndexFactor>::eliminateSymbolic_(Conditionals& conditionals) const {
static const bool debug = false;
if(debug) cout << "ETree: eliminating " << this->key_ << endl;
FastSet<Index> variables;
BOOST_FOREACH(const sharedFactor& factor, factors_) {
variables.insert(factor->begin(), factor->end());
}
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
sharedFactor factor(child->eliminate_(conditionals));
variables.insert(factor->begin(), factor->end());
}
conditionals[this->key_] = IndexConditional::FromRange(variables.begin(), variables.end(), 1);
variables.erase(variables.begin());
if(debug) cout << "Eliminated " << this->key_ << " to get:\n";
if(debug) conditionals[this->key_]->print("Conditional: ");
return variables;
}
/* ************************************************************************* */
// This non-specialized version cannot be called.
template<class FACTOR> FastSet<Index>
EliminationTree<FACTOR>::eliminateSymbolic_(Conditionals& conditionals) const {
throw invalid_argument("symbolic eliminate should never be called from a non-IndexFactor EliminationTree");
return FastSet<Index>();
}
/* ************************************************************************* */
template<class FACTOR>
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
@ -126,8 +93,9 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& struc
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex& structure) {
typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
const FactorGraph<DERIVEDFACTOR>& factorGraph,
const VariableIndex& structure) {
static const bool debug = false;
@ -218,12 +186,12 @@ bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& ex
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTree<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminate() const {
EliminationTree<FACTOR>::eliminate(Eliminate function) const {
// call recursive routine
tic(1, "ET recursive eliminate");
Conditionals conditionals(this->key_ + 1);
(void)eliminate_(conditionals);
(void)eliminate_(function, conditionals);
toc(1, "ET recursive eliminate");
// Add conditionals to BayesNet
@ -238,28 +206,4 @@ EliminationTree<FACTOR>::eliminate() const {
return bayesNet;
}
/* ************************************************************************* */
// Specialization for symbolic elimination that calls the optimized eliminateSymbolic_
template<>
inline EliminationTree<IndexFactor>::BayesNet::shared_ptr
EliminationTree<IndexFactor>::eliminate() const {
// call recursive routine
tic(1, "ET recursive eliminate");
Conditionals conditionals(this->key_ + 1);
(void)eliminateSymbolic_(conditionals);
toc(1, "ET recursive eliminate");
// Add conditionals to BayesNet
tic(2, "assemble BayesNet");
BayesNet::shared_ptr bayesNet(new BayesNet);
BOOST_FOREACH(const BayesNet::sharedConditional& conditional, conditionals) {
if(conditional)
bayesNet->push_back(conditional);
}
toc(2, "assemble BayesNet");
return bayesNet;
}
}

13
gtsam/inference/EliminationTree.h
View File

@ -42,6 +42,9 @@ public:
typedef boost::shared_ptr<EliminationTree<FACTOR> > shared_ptr;
typedef gtsam::BayesNet<typename FACTOR::ConditionalType> BayesNet;
/** typedef for an eliminate subroutine */
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate;
private:
typedef FastList<sharedFactor> Factors;
@ -70,13 +73,7 @@ private:
/**
* Recursive routine that eliminates the factors arranged in an elimination tree
*/
sharedFactor eliminate_(Conditionals& conditionals) const;
/**
* Special optimized eliminate for symbolic factors. Will not compile if
* called in a non-IndexFactor EliminationTree.
*/
FastSet<Index> eliminateSymbolic_(Conditionals& conditionals) const;
sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const;
// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeTester;
@ -101,7 +98,7 @@ public:
bool equals(const EliminationTree& other, double tol = 1e-9) const;
/** Eliminate the factors to a Bayes Net */
typename BayesNet::shared_ptr eliminate() const;
typename BayesNet::shared_ptr eliminate(Eliminate function) const;
};
}

144
gtsam/inference/Factor-inl.h
View File

@ -18,99 +18,81 @@
#pragma once
#include <gtsam/inference/Factor.h>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <gtsam/inference/Factor.h>
namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const Factor<KEY>& f) : keys_(f.keys_) { assertInvariants(); }
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const Factor<KEY>& f) :
keys_(f.keys_) {
assertInvariants();
}
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const ConditionalType& c) : keys_(c.keys()) { assertInvariants(); }
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const ConditionalType& c) :
keys_(c.keys()) {
assertInvariants();
}
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::assertInvariants() const {
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::assertInvariants() const {
#ifndef NDEBUG
// Check that keys are all unique
std::multiset<Key> nonunique(keys_.begin(), keys_.end());
std::set<Key> unique(keys_.begin(), keys_.end());
assert(nonunique.size() == unique.size() && std::equal(nonunique.begin(), nonunique.end(), unique.begin()));
// Check that keys are all unique
std::multiset<Key> nonunique(keys_.begin(), keys_.end());
std::set<Key> unique(keys_.begin(), keys_.end());
assert(nonunique.size() == unique.size() && std::equal(nonunique.begin(), nonunique.end(), unique.begin()));
#endif
}
}
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::print(const std::string& s) const {
std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << key;
std::cout << std::endl;
}
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::print(const std::string& s) const {
std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_)
std::cout << " " << key;
std::cout << std::endl;
}
/* ************************************************************************* */
template<typename KEY>
bool Factor<KEY>::equals(const This& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
template<typename KEY>
bool Factor<KEY>::equals(const This& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
template<typename KEY>
template<class DERIVED>
typename DERIVED::shared_ptr Factor<KEY>::Combine(const FactorGraph<DERIVED>& factors, const FastMap<Key, std::vector<Key> >& variableSlots) {
typedef const FastMap<Key, std::vector<Key> > VariableSlots;
typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) FirstGetter;
typedef boost::transform_iterator<
FirstGetter, typename VariableSlots::const_iterator,
KEY, KEY> IndexIterator;
FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1));
IndexIterator keysBegin(variableSlots.begin(), firstGetter);
IndexIterator keysEnd(variableSlots.end(), firstGetter);
return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd));
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
template<typename KEY>
template<class COND>
typename COND::shared_ptr Factor<KEY>::eliminateFirst() {
assert(!keys_.empty());
assertInvariants();
KEY eliminated = keys_.front();
keys_.erase(keys_.begin());
assertInvariants();
return typename COND::shared_ptr(new COND(eliminated, keys_));
}
/* ************************************************************************* */
template<typename KEY>
template<class CONDITIONAL>
typename CONDITIONAL::shared_ptr Factor<KEY>::eliminateFirst() {
assert(!keys_.empty());
assertInvariants();
KEY eliminated = keys_.front();
keys_.erase(keys_.begin());
assertInvariants();
return typename CONDITIONAL::shared_ptr(new CONDITIONAL(eliminated, keys_));
}
/* ************************************************************************* */
template<typename KEY>
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr Factor<KEY>::eliminate(size_t nrFrontals) {
assert(keys_.size() >= nrFrontals);
assertInvariants();
typename BayesNet<CONDITIONAL>::shared_ptr fragment(new BayesNet<CONDITIONAL>());
const_iterator nextFrontal = this->begin();
for(KEY n = 0; n < nrFrontals; ++n, ++nextFrontal)
fragment->push_back(CONDITIONAL::FromRange(
nextFrontal, const_iterator(this->end()), 1));
if(nrFrontals > 0)
keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents());
assertInvariants();
return fragment;
}
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(KEY& key, keys_) { key = inversePermutation[key]; }
assertInvariants();
}
/* ************************************************************************* */
template<typename KEY>
template<class COND>
typename BayesNet<COND>::shared_ptr Factor<KEY>::eliminate(size_t nrFrontals) {
assert(keys_.size() >= nrFrontals);
assertInvariants();
typename BayesNet<COND>::shared_ptr fragment(new BayesNet<COND> ());
const_iterator it = this->begin();
for (KEY n = 0; n < nrFrontals; ++n, ++it)
fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1));
if (nrFrontals > 0) keys_.assign(fragment->back()->beginParents(),
fragment->back()->endParents());
assertInvariants();
return fragment;
}
#endif
}

17
gtsam/inference/Factor.h
View File

@ -21,15 +21,14 @@
#pragma once
#include <vector>
#include <map>
#include <set>
#include <vector>
#include <boost/utility.hpp> // for noncopyable
#include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/inference.h>
namespace gtsam {
@ -117,10 +116,7 @@ public:
keys_.push_back(key);
assertInvariants(); }
/** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVED>
static typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, const FastMap<Key, std::vector<Key> >& variableSlots);
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
@ -133,12 +129,7 @@ public:
*/
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
/**
* Permutes the factor, but for efficiency requires the permutation
* to already be inverted.
*/
void permuteWithInverse(const Permutation& inversePermutation);
#endif
/** iterators */
const_iterator begin() const { return keys_.begin(); }

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

@ -30,7 +30,10 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/graph-inl.h>
@ -112,5 +115,21 @@ namespace gtsam {
return fg;
}
/* ************************************************************************* */
template<class DERIVED, class KEY>
typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots) {
typedef const FastMap<KEY, std::vector<KEY> > VariableSlots;
typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1))
FirstGetter;
typedef boost::transform_iterator<FirstGetter,
typename VariableSlots::const_iterator, KEY, KEY> IndexIterator;
FirstGetter firstGetter(boost::lambda::bind(
&VariableSlots::value_type::first, boost::lambda::_1));
IndexIterator keysBegin(variableSlots.begin(), firstGetter);
IndexIterator keysEnd(variableSlots.end(), firstGetter);
return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd));
}
/* ************************************************************************* */
} // namespace gtsam

15
gtsam/inference/FactorGraph.h
View File

@ -24,8 +24,10 @@
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/graph.h>
@ -47,6 +49,14 @@ namespace gtsam {
typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
/** typedef for elimination result */
typedef std::pair<
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr,
typename FACTOR::shared_ptr> EliminationResult;
/** typedef for an eliminate subroutine */
typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
protected:
/** Collection of factors */
@ -176,6 +186,11 @@ namespace gtsam {
}
}; // FactorGraph
/** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVED, class KEY>
typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots);
/**
* static function that combines two factor graphs
* @param const &fg1 Linear factor graph

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

@ -48,34 +48,37 @@ void GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::replaceFactors(const typen
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename JUNCTIONTREE::BayesTree::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate() const {
typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(new typename JUNCTIONTREE::BayesTree);
bayesTree->insert(junctionTree_->eliminate());
return bayesTree;
typename JUNCTIONTREE::BayesTree::shared_ptr GenericMultifrontalSolver<
FACTOR, JUNCTIONTREE>::eliminate(
typename FactorGraph<FACTOR>::Eliminate function) const {
typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(
new typename JUNCTIONTREE::BayesTree);
bayesTree->insert(junctionTree_->eliminate(function));
return bayesTree;
}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FactorGraph<FACTOR>::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::jointFactorGraph(const std::vector<Index>& js) const {
typename FactorGraph<FACTOR>::shared_ptr GenericMultifrontalSolver<FACTOR,
JUNCTIONTREE>::jointFactorGraph(const std::vector<Index>& js,
Eliminate function) const {
// We currently have code written only for computing the
// We currently have code written only for computing the
if(js.size() != 2)
throw domain_error(
"*MultifrontalSolver::joint(js) currently can only compute joint marginals\n"
"for exactly two variables. You can call marginal to compute the\n"
"marginal for one variable. *SequentialSolver::joint(js) can compute the\n"
"joint marginal over any number of variables, so use that if necessary.\n");
if (js.size() != 2) throw domain_error(
"*MultifrontalSolver::joint(js) currently can only compute joint marginals\n"
"for exactly two variables. You can call marginal to compute the\n"
"marginal for one variable. *SequentialSolver::joint(js) can compute the\n"
"joint marginal over any number of variables, so use that if necessary.\n");
return eliminate()->joint(js[0], js[1]);
return eliminate(function)->joint(js[0], js[1], function);
}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginalFactor(Index j) const {
return eliminate()->marginalFactor(j);
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginalFactor(
Index j, Eliminate function) const {
return eliminate(function)->marginalFactor(j, function);
}
}

16
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -44,9 +44,11 @@ protected:
// Junction tree that performs elimination.
typename JUNCTIONTREE::shared_ptr junctionTree_;
public:
typedef typename FactorGraph<FACTOR>::shared_ptr sharedGraph;
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate;
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which does the symbolic elimination, identifies the cliques,
@ -59,33 +61,35 @@ public:
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
GenericMultifrontalSolver(const sharedGraph& factorGraph,
const VariableIndex::shared_ptr& variableIndex);
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
* such as during relinearization or adjusting of noise models.
*/
void replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
void replaceFactors(const sharedGraph& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename JUNCTIONTREE::BayesTree::shared_ptr eliminate() const;
typename JUNCTIONTREE::BayesTree::shared_ptr
eliminate(Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a factor
* graph.
*/
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
sharedGraph jointFactorGraph(const std::vector<Index>& js, Eliminate function) const;
/**
* Compute the marginal 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) const;
typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const;
};

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

@ -22,6 +22,7 @@
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>
@ -48,9 +49,9 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::print(const std::string& s) const {
this->factors_->print(s+" factors:");
this->structure_->print(s+" structure:\n");
this->eliminationTree_->print(s+" etree:");
this->factors_->print(s + " factors:");
this->structure_->print(s + " structure:\n");
this->eliminationTree_->print(s + " etree:");
}
/* ************************************************************************* */
@ -77,15 +78,17 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
GenericSequentialSolver<FACTOR>::eliminate() const {
return eliminationTree_->eliminate();
GenericSequentialSolver<FACTOR>::eliminate(
typename EliminationTree<FACTOR>::Eliminate function) const {
return eliminationTree_->eliminate(function);
}
/* ************************************************************************* */
template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointFactorGraph(
const std::vector<Index>& js) const {
const std::vector<Index>& js,
typename EliminationTree<FACTOR>::Eliminate function) const {
// Compute a COLAMD permutation with the marginal variable constrained to the end.
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(
@ -100,7 +103,7 @@ namespace gtsam {
// Eliminate all variables
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr bayesNet(
EliminationTree<FACTOR>::Create(*factors_)->eliminate());
EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
// Undo the permuation on the original factors and on the structure.
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
@ -125,13 +128,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename FACTOR::shared_ptr //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j) const {
GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
typename EliminationTree<FACTOR>::Eliminate function) const {
// Create a container for the one variable index
vector<Index> js(1);
js[0] = j;
// Call joint and return the only factor in the factor graph it returns
return (*this->jointFactorGraph(js))[0];
return (*this->jointFactorGraph(js, function))[0];
}
} // namespace gtsam

19
gtsam/inference/GenericSequentialSolver.h
View File

@ -27,7 +27,8 @@
namespace gtsam {
template<class FACTOR>
class GenericSequentialSolver : public Testable<GenericSequentialSolver<FACTOR> > {
class GenericSequentialSolver: public Testable<
GenericSequentialSolver<FACTOR> > {
protected:
@ -57,11 +58,11 @@ namespace gtsam {
const typename FactorGraph<FACTOR>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex);
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
/** Test whether is equal to another */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/** Test whether is equal to another */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/**
* Replace the factor graph with a new one having the same structure. The
@ -76,7 +77,7 @@ namespace gtsam {
* to recursively eliminate.
*/
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr
eliminate() const;
eliminate(typename EliminationTree<FACTOR>::Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
@ -84,13 +85,15 @@ namespace gtsam {
* graph.
*/
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(
const std::vector<Index>& js) const;
const std::vector<Index>& js,
typename EliminationTree<FACTOR>::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) const;
typename FACTOR::shared_ptr marginalFactor(Index j,
typename EliminationTree<FACTOR>::Eliminate function) const;
}; // GenericSequentialSolver

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

@ -41,19 +41,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
template<class FG> void ISAM<CONDITIONAL>::update_internal(
const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) {
// Remove the contaminated part of the Bayes tree
BayesNet<CONDITIONAL> bn;
removeTop(newFactors.keys(), bn, orphans);
FACTORGRAPH factors(bn);
FG factors(bn);
// add the factors themselves
factors.push_back(newFactors);
// eliminate into a Bayes net
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::FactorType>(factors).eliminate();
GenericSequentialSolver<typename CONDITIONAL::FactorType> solver(factors);
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = solver.eliminate(function);
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
@ -61,17 +62,17 @@ namespace gtsam {
this->insert(*rit);
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
BOOST_FOREACH(sharedClique orphan, orphans)
this->insert(orphan);
}
}
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update(const FACTORGRAPH& newFactors) {
template<class FG>
void ISAM<CONDITIONAL>::update(const FG& newFactors,
typename FG::Eliminate function) {
Cliques orphans;
this->update_internal(newFactors, orphans);
this->update_internal(newFactors, orphans, function);
}
}

10
gtsam/inference/ISAM.h
View File

@ -52,10 +52,12 @@ namespace gtsam {
/**
* iSAM. (update_internal provides access to list of orphans for drawing purposes)
*/
template<class FACTORGRAPH>
void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans);
template<class FACTORGRAPH>
void update(const FACTORGRAPH& newFactors);
template<class FG>
void update_internal(const FG& newFactors, Cliques& orphans,
typename FG::Eliminate function);
template<class FG>
void update(const FG& newFactors, typename FG::Eliminate function);
}; // ISAM

33
gtsam/inference/IndexConditional.cpp
View File

@ -41,4 +41,37 @@ namespace gtsam {
#endif
}
/* ************************************************************************* */
bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
}
}
assertInvariants();
return parentChanged;
}
/* ************************************************************************* */
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
#endif
BOOST_FOREACH(Index& key, keys_)
key = inversePermutation[key];
assertInvariants();
}
/* ************************************************************************* */
}

14
gtsam/inference/IndexConditional.h
View File

@ -18,8 +18,10 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/Permutation.h>
namespace gtsam {
@ -80,6 +82,18 @@ namespace gtsam {
/** Convert to a factor */
IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); }
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/**
* Permutes the Conditional, but for efficiency requires the permutation
* to already be inverted.
*/
void permuteWithInverse(const Permutation& inversePermutation);
};
}

107
gtsam/inference/IndexFactor.cpp
View File

@ -16,76 +16,57 @@
* @created Oct 17, 2010
*/
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/VariableSlots.h>
using namespace std;
namespace gtsam {
template class Factor<Index>;
template class Factor<Index> ;
/* ************************************************************************* */
void IndexFactor::assertInvariants() const {
Base::assertInvariants();
#ifndef NDEBUG
#ifndef GTSAM_NO_ENFORCE_ORDERING
std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
assert(uniqueSorted.size() == keys_.size() && std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
/* ************************************************************************* */
void IndexFactor::assertInvariants() const {
Base::assertInvariants();
//#ifndef NDEBUG
//#ifndef GTSAM_NO_ENFORCE_ORDERING
// std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
// assert(uniqueSorted.size() == keys_.size() && std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
//#endif
//#endif
}
/* ************************************************************************* */
IndexFactor::IndexFactor(const IndexConditional& c) :
Base(c) {
assertInvariants();
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() {
boost::shared_ptr<IndexConditional> result(Base::eliminateFirst<
IndexConditional>());
assertInvariants();
return result;
}
/* ************************************************************************* */
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate(
size_t nrFrontals) {
boost::shared_ptr<BayesNet<IndexConditional> > result(Base::eliminate<
IndexConditional>(nrFrontals));
assertInvariants();
return result;
}
#endif
#endif
}
/* ************************************************************************* */
IndexFactor::IndexFactor(const IndexConditional& c): Base(c) {
assertInvariants();
}
/* ************************************************************************* */
pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> IndexFactor::CombineAndEliminate(
const FactorGraph<This>& factors, size_t nrFrontals) {
FastSet<Index> variables;
BOOST_FOREACH(const shared_ptr& factor, factors) {
BOOST_FOREACH(Index var, *factor) {
variables.insert(var); } }
if(variables.size() < 1)
throw invalid_argument("IndexFactor::CombineAndEliminate called on factors with zero total variables.");
pair<BayesNet<ConditionalType>::shared_ptr, shared_ptr> result;
result.first.reset(new BayesNet<IndexConditional>());
FastSet<Index>::const_iterator var;
for(var = variables.begin(); result.first->size() < nrFrontals; ++var)
result.first->push_back(IndexConditional::FromRange(var, variables.end(), 1));
result.second.reset(new IndexFactor(var, variables.end()));
return result;
}
/* ************************************************************************* */
IndexFactor::shared_ptr IndexFactor::Combine(
const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots) {
IndexFactor::shared_ptr combined(Base::Combine<This>(factors, variableSlots));
combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() {
boost::shared_ptr<IndexConditional> result(Base::eliminateFirst<IndexConditional>());
assertInvariants();
return result;
}
/* ************************************************************************* */
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate(size_t nrFrontals) {
boost::shared_ptr<BayesNet<IndexConditional> > result(Base::eliminate<IndexConditional>(nrFrontals));
assertInvariants();
return result;
}
}
/* ************************************************************************* */
void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys_)
key = inversePermutation[key];
assertInvariants();
}
/* ************************************************************************* */
} // gtsam

146
gtsam/inference/IndexFactor.h
View File

@ -19,93 +19,111 @@
#pragma once
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Permutation.h>
namespace gtsam {
// Forward declaration of IndexConditional
class IndexConditional;
// Forward declaration of IndexConditional
class IndexConditional;
/**
* IndexFactor serves two purposes. It is the base class for all linear
* factors (GaussianFactor, JacobianFactor, HessianFactor), and also
* functions as a symbolic factor with Index keys, used to do symbolic
* elimination by JunctionTree.
*
* This class provides symbolic elimination, via the CombineAndEliminate
* function. Combine and eliminate can also be called separately, but for
* this and derived classes calling them separately generally does extra
* work.
*
* It derives from Factor with a key type of Index, which is an unsigned
* integer.
*/
class IndexFactor : public Factor<Index> {
/**
* IndexFactor serves two purposes. It is the base class for all linear
* factors (GaussianFactor, JacobianFactor, HessianFactor), and also
* functions as a symbolic factor with Index keys, used to do symbolic
* elimination by JunctionTree.
*
* It derives from Factor with a key type of Index, an unsigned integer.
*/
class IndexFactor: public Factor<Index> {
protected:
protected:
// Internal function for checking class invariants (sorted keys for this factor)
void assertInvariants() const;
// Internal function for checking class invariants (sorted keys for this factor)
void assertInvariants() const;
public:
public:
typedef IndexFactor This;
typedef Factor<Index> Base;
typedef IndexFactor This;
typedef Factor<Index> Base;
/** Elimination produces an IndexConditional */
typedef IndexConditional ConditionalType;
/** Elimination produces an IndexConditional */
typedef IndexConditional ConditionalType;
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactor> shared_ptr;
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactor> shared_ptr;
/** Copy constructor */
IndexFactor(const This& f) : Base(f) { assertInvariants(); }
/** Copy constructor */
IndexFactor(const This& f) :
Base(f) {
assertInvariants();
}
/** Construct from derived type */
IndexFactor(const IndexConditional& c);
/** Construct from derived type */
IndexFactor(const IndexConditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey, KeyIterator endKey) :
Base(beginKey, endKey) { assertInvariants(); }
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey,
KeyIterator endKey) :
Base(beginKey, endKey) {
assertInvariants();
}
/** Default constructor for I/O */
IndexFactor() { assertInvariants(); }
/** Default constructor for I/O */
IndexFactor() {
assertInvariants();
}
/** Construct unary factor */
IndexFactor(Index j) : Base(j) { assertInvariants(); }
/** Construct unary factor */
IndexFactor(Index j) :
Base(j) {
assertInvariants();
}
/** Construct binary factor */
IndexFactor(Index j1, Index j2) : Base(j1, j2) { assertInvariants(); }
/** Construct binary factor */
IndexFactor(Index j1, Index j2) :
Base(j1, j2) {
assertInvariants();
}
/** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) { assertInvariants(); }
/** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) :
Base(j1, j2, j3) {
assertInvariants();
}
/** Construct 4-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) { assertInvariants(); }
/** Construct 4-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4) :
Base(j1, j2, j3, j4) {
assertInvariants();
}
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) : Base(js) { assertInvariants(); }
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) :
Base(js) {
assertInvariants();
}
/**
* Combine and eliminate several factors.
*/
static std::pair<BayesNet<ConditionalType>::shared_ptr, shared_ptr> CombineAndEliminate(
const FactorGraph<This>& factors, size_t nrFrontals=1);
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
boost::shared_ptr<ConditionalType> eliminateFirst();
/** Create a combined joint factor (new style for EliminationTree). */
static shared_ptr
Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots);
/** eliminate the first nrFrontals frontal variables.*/
boost::shared_ptr<BayesNet<ConditionalType> > eliminate(size_t nrFrontals =
1);
#endif
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
boost::shared_ptr<ConditionalType> eliminateFirst();
/**
* Permutes the factor, but for efficiency requires the permutation
* to already be inverted.
*/
void permuteWithInverse(const Permutation& inversePermutation);
/**
* eliminate the first nrFrontals frontal variables.
*/
boost::shared_ptr<BayesNet<ConditionalType> > eliminate(size_t nrFrontals = 1);
virtual ~IndexFactor() {
}
};
}; // IndexFactor
}

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

@ -40,10 +40,11 @@ namespace gtsam {
void JunctionTree<FG>::construct(const FG& fg, const VariableIndex& variableIndex) {
tic(1, "JT Constructor");
tic(1, "JT symbolic ET");
const typename EliminationTree<IndexFactor>::shared_ptr symETree(EliminationTree<IndexFactor>::Create(fg, variableIndex));
const typename EliminationTree<IndexFactor>::shared_ptr symETree =
EliminationTree<IndexFactor>::Create(fg, variableIndex);
toc(1, "JT symbolic ET");
tic(2, "JT symbolic eliminate");
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate();
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
toc(2, "JT symbolic eliminate");
tic(3, "symbolic BayesTree");
SymbolicBayesTree sbt(*sbn);
@ -148,9 +149,11 @@ namespace gtsam {
}
/* ************************************************************************* */
template <class FG>
pair<typename JunctionTree<FG>::BayesTree::sharedClique, typename FG::sharedFactor>
JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current, bool cache) const {
template<class FG>
pair<typename JunctionTree<FG>::BayesTree::sharedClique,
typename FG::sharedFactor> JunctionTree<FG>::eliminateOneClique(
typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& current, bool cache) const {
FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size());
@ -160,7 +163,7 @@ namespace gtsam {
list<typename BayesTree::sharedClique> children;
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
eliminateOneClique(child, cache));
eliminateOneClique(function, child, cache));
children.push_back(tree_factor.first);
fg.push_back(tree_factor.second);
}
@ -171,8 +174,10 @@ 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 BayesNet<typename FG::FactorType::ConditionalType>::shared_ptr, typename FG::sharedFactor> eliminated(
FG::FactorType::CombineAndEliminate(fg, current->frontal.size()));
pair<
typename BayesNet<typename FG::FactorType::ConditionalType>::shared_ptr,
typename FG::sharedFactor> eliminated(function(fg,
current->frontal.size()));
toc(2, "CombineAndEliminate");
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
@ -193,17 +198,19 @@ namespace gtsam {
}
/* ************************************************************************* */
template <class FG>
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate(bool cache) const {
if(this->root()) {
tic(2,"JT eliminate");
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root(), cache);
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();
}
template<class FG>
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate(
typename FG::Eliminate function, bool cache) const {
if (this->root()) {
tic(2, "JT eliminate");
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret =
this->eliminateOneClique(function, this->root(), cache);
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();
}
} //namespace gtsam

7
gtsam/inference/JunctionTree.h
View File

@ -27,6 +27,7 @@
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/ClusterTree.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {
@ -64,7 +65,8 @@ namespace gtsam {
// recursive elimination function
std::pair<typename BayesTree::sharedClique, typename FG::sharedFactor>
eliminateOneClique(const boost::shared_ptr<const Clique>& clique, bool cache=false) const;
eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique, bool cache = false) const;
// internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex);
@ -80,7 +82,8 @@ namespace gtsam {
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
// eliminate the factors in the subgraphs
typename BayesTree::sharedClique eliminate(bool cache=false) const;
typename BayesTree::sharedClique eliminate(typename FG::Eliminate function,
bool cache = false) const;
}; // JunctionTree

46
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -16,19 +16,15 @@
* Author: Frank Dellaert
*/
#include <iostream>
#include <fstream>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/EliminationTree-inl.h>
using namespace std;
namespace gtsam {
using namespace std;
// Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<IndexFactor>;
template class BayesNet<IndexConditional>;
@ -71,13 +67,37 @@ namespace gtsam {
return keys;
}
/* ************************************************************************* */
IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
std::vector<Index> >& variableSlots) {
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors,
variableSlots));
// combined->assertInvariants();
return combined;
}
// /* ************************************************************************* */
// SymbolicBayesNet
// SymbolicFactorGraph::eliminateFrontals(const Ordering& ordering)
// {
// return Inference::Eliminate(ordering);
// }
/* ************************************************************************* */
pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> //
EliminateSymbolic(const FactorGraph<IndexFactor>& factors, size_t nrFrontals) {
FastSet<Index> keys;
BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors)
BOOST_FOREACH(Index var, *factor)
keys.insert(var);
if (keys.size() < 1) throw invalid_argument(
"IndexFactor::CombineAndEliminate called on factors with no variables.");
pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> result;
result.first.reset(new BayesNet<IndexConditional> ());
FastSet<Index>::const_iterator it;
for (it = keys.begin(); result.first->size() < nrFrontals; ++it)
result.first->push_back(IndexConditional::FromRange(it, keys.end(), 1));
result.second.reset(new IndexFactor(it, keys.end()));
return result;
}
/* ************************************************************************* */
}

101
gtsam/inference/SymbolicFactorGraph.h
View File

@ -18,72 +18,75 @@
#pragma once
#include <string>
#include <list>
#include <gtsam/base/types.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/EliminationTree.h>
namespace gtsam {
typedef BayesNet<IndexConditional> SymbolicBayesNet;
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
typedef BayesNet<IndexConditional> SymbolicBayesNet;
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
/** Symbolic IndexFactor Graph */
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
/** Symbolic IndexFactor Graph */
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
public:
public:
/** Construct empty factor graph */
SymbolicFactorGraph() {}
/** Construct empty factor graph */
SymbolicFactorGraph() {
}
/** Construct from a BayesNet */
SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet);
/** Construct from a BayesNet */
SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet);
/** Push back unary factor */
void push_factor(Index key);
/** Push back unary factor */
void push_factor(Index key);
/** Push back binary factor */
void push_factor(Index key1, Index key2);
/** Push back binary factor */
void push_factor(Index key1, Index key2);
/** Push back ternary factor */
void push_factor(Index key1, Index key2, Index key3);
/** Push back ternary factor */
void push_factor(Index key1, Index key2, Index key3);
/** Push back 4-way factor */
void push_factor(Index key1, Index key2, Index key3, Index key4);
/** Push back 4-way factor */
void push_factor(Index key1, Index key2, Index key3, Index key4);
/**
* Construct from a factor graph of any type
*/
template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
/**
* Construct from a factor graph of any type
*/
template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
/**
* Return the set of variables involved in the factors (computes a set
* union).
*/
FastSet<Index> keys() const;
/**
* Return the set of variables involved in the factors (computes a set
* union).
*/
FastSet<Index> keys() const;
};
/**
* Same as eliminate in the SymbolicFactorGraph case
*/
// SymbolicBayesNet eliminateFrontals(const Ordering& ordering);
};
/** Create a combined joint factor (new style for EliminationTree). */
IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
std::vector<Index> >& variableSlots);
/* Template function implementation */
template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
if(fg[i]) {
IndexFactor::shared_ptr factor(new IndexFactor(*fg[i]));
push_back(factor);
} else
push_back(IndexFactor::shared_ptr());
}
}
/**
* CombineAndEliminate provides symbolic elimination.
* Combine and eliminate can also be called separately, but for this and
* derived classes calling them separately generally does extra work.
*/
std::pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr>
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
/* Template function implementation */
template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
if (fg[i]) {
IndexFactor::shared_ptr factor(new IndexFactor(*fg[i]));
push_back(factor);
} else
push_back(IndexFactor::shared_ptr());
}
}
} // namespace gtsam

3
gtsam/inference/tests/testEliminationTree.cpp
View File

@ -89,7 +89,8 @@ TEST(EliminationTree, eliminate )
fg.push_factor(3, 4);
// eliminate
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(
&EliminateSymbolic);
CHECK(assert_equal(expected,actual));
}

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

@ -82,9 +82,10 @@ TEST( JunctionTree, eliminate)
fg.push_factor(x3,x4);
SymbolicJunctionTree jt(fg);
SymbolicBayesTree::sharedClique actual = jt.eliminate();
SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic);
BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate(
&EliminateSymbolic));
SymbolicBayesTree expected(bn);
// cout << "BT from JT:\n";

5
gtsam/inference/tests/testSymbolicBayesNet.cpp
View File

@ -21,8 +21,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
//#define GTSAM_MAGIC_KEY
#include <gtsam/inference/IndexConditional.h>
#ifdef ALL
#include <gtsam/inference/SymbolicFactorGraph.h>
using namespace std;
@ -100,6 +100,7 @@ TEST( SymbolicBayesNet, combine )
CHECK(assert_equal(expected,p_ABC));
}
#endif
/* ************************************************************************* */
int main() {

7
gtsam/inference/tests/testSymbolicFactor.cpp
View File

@ -45,6 +45,7 @@ TEST(SymbolicFactor, constructor) {
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
TEST(SymbolicFactor, eliminate) {
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
IndexFactor actual(keys.begin(), keys.end());
@ -62,9 +63,9 @@ TEST(SymbolicFactor, eliminate) {
CHECK(assert_equal(**fragmentCond++, *expected1));
CHECK(assert_equal(**fragmentCond++, *expected2));
}
#endif
/* ************************************************************************* */
TEST(SymbolicFactor, CombineAndEliminate) {
TEST(SymbolicFactor, EliminateSymbolic) {
SymbolicFactorGraph factors;
factors.push_factor(2,4,6);
factors.push_factor(1,2,5);
@ -88,7 +89,7 @@ TEST(SymbolicFactor, CombineAndEliminate) {
BayesNet<IndexConditional>::shared_ptr actual_bn;
IndexFactor::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) = IndexFactor::CombineAndEliminate(factors, 4);
boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4);
CHECK(assert_equal(expected_bn, *actual_bn));
CHECK(assert_equal(expected_factor, *actual_factor));

184
gtsam/linear/GaussianFactor.cpp
View File

@ -16,188 +16,16 @@
* @author Richard Roberts, Christian Potthast
*/
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <boost/shared_ptr.hpp>
#include <stdexcept>
using namespace std;
#include <gtsam/linear/GaussianConditional.h>
namespace gtsam {
/* ************************************************************************* */
GaussianFactor::GaussianFactor(const GaussianConditional& c) : IndexFactor(c) {}
using namespace std;
/* ************************************************************************* */
pair<GaussianBayesNet::shared_ptr, GaussianFactor::shared_ptr> GaussianFactor::CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals, SolveMethod solveMethod) {
const bool debug = ISDEBUG("GaussianFactor::CombineAndEliminate");
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but Cholesky cannot.
// Decide whether to use QR or Cholesky
bool useQR;
if(solveMethod == SOLVE_QR) {
useQR = true;
} else if(solveMethod == SOLVE_PREFER_CHOLESKY) {
// Check if any JacobianFactors have constrained noise models.
useQR = false;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(jacobianFactor && jacobianFactor->get_model()->isConstrained()) {
useQR = true;
break;
}
}
}
// Convert all factors to the appropriate type and call the type-specific CombineAndEliminate.
if(useQR) {
if(debug) cout << "Using QR:";
tic(1, "convert to Jacobian");
FactorGraph<JacobianFactor> jacobianFactors;
jacobianFactors.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
if(factor) {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(jacobianFactor) {
jacobianFactors.push_back(jacobianFactor);
if(debug) jacobianFactor->print("Existing JacobianFactor: ");
} else {
HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast<HessianFactor>(factor));
if(!hessianFactor) throw std::invalid_argument(
"In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor.");
jacobianFactors.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*hessianFactor)));
if(debug) {
cout << "Converted HessianFactor to JacobianFactor:\n";
hessianFactor->print("HessianFactor: ");
jacobianFactors.back()->print("JacobianFactor: ");
}
}
}
}
toc(1, "convert to Jacobian");
tic(2, "Jacobian CombineAndEliminate");
pair<GaussianBayesNet::shared_ptr, GaussianFactor::shared_ptr> ret(
JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals));
toc(2, "Jacobian CombineAndEliminate");
return ret;
} else {
const bool checkCholesky = ISDEBUG("GaussianFactor::CombineAndEliminate Check Cholesky");
// FactorGraph<HessianFactor> hessianFactors;
// tic(1, "convert to Hessian");
// hessianFactors.reserve(factors.size());
// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
// if(factor) {
// HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast<HessianFactor>(factor));
// if(hessianFactor)
// hessianFactors.push_back(hessianFactor);
// else {
// JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
// if(!jacobianFactor) throw std::invalid_argument(
// "In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor.");
// HessianFactor::shared_ptr convertedHessianFactor;
// try {
// convertedHessianFactor.reset(new HessianFactor(*jacobianFactor));
// if(checkCholesky)
// if(!assert_equal(HessianFactor(*jacobianFactor), HessianFactor(JacobianFactor(*convertedHessianFactor)), 1e-3))
// throw runtime_error("Conversion between Jacobian and Hessian incorrect");
// } catch(const exception& e) {
// cout << "Exception converting to Hessian: " << e.what() << endl;
// jacobianFactor->print("jacobianFactor: ");
// convertedHessianFactor->print("convertedHessianFactor: ");
// SETDEBUG("choleskyPartial", true);
// SETDEBUG("choleskyCareful", true);
// HessianFactor(JacobianFactor(*convertedHessianFactor)).print("Jacobian->Hessian->Jacobian->Hessian: ");
// throw;
// }
// hessianFactors.push_back(convertedHessianFactor);
// }
// }
// }
// toc(1, "convert to Hessian");
pair<GaussianBayesNet::shared_ptr, GaussianFactor::shared_ptr> ret;
try {
tic(2, "Hessian CombineAndEliminate");
ret = HessianFactor::CombineAndEliminate(factors, nrFrontals);
toc(2, "Hessian CombineAndEliminate");
} catch(const exception& e) {
cout << "Exception in HessianFactor::CombineAndEliminate: " << e.what() << endl;
SETDEBUG("HessianFactor::CombineAndEliminate", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("choleskyPartial", true);
factors.print("Combining factors: ");
HessianFactor::CombineAndEliminate(factors, nrFrontals);
throw;
}
if(checkCholesky) {
pair<GaussianBayesNet::shared_ptr, GaussianFactor::shared_ptr> expected;
FactorGraph<JacobianFactor> jacobianFactors;
try {
// Compare with QR
jacobianFactors.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
if(factor) {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(jacobianFactor)
jacobianFactors.push_back(jacobianFactor);
else {
HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast<HessianFactor>(factor));
if(!hessianFactor) throw std::invalid_argument(
"In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor.");
JacobianFactor::shared_ptr convertedJacobianFactor(new JacobianFactor(*hessianFactor));
// if(!assert_equal(*hessianFactor, HessianFactor(*convertedJacobianFactor), 1e-3))
// throw runtime_error("Conversion between Jacobian and Hessian incorrect");
jacobianFactors.push_back(convertedJacobianFactor);
}
}
}
expected = JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals);
} catch(...) {
cout << "Exception in QR" << endl;
throw;
}
HessianFactor actual_factor(*ret.second);
HessianFactor expected_factor(*expected.second);
if(!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(expected_factor, actual_factor, 1.0)) {
cout << "Cholesky and QR do not agree" << endl;
SETDEBUG("HessianFactor::CombineAndEliminate", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobianFactors.print("Jacobian Factors: ");
JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals);
HessianFactor::CombineAndEliminate(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("Cholesky and QR do not agree");
}
}
return ret;
}
}
/* ************************************************************************* */
GaussianFactor::GaussianFactor(const GaussianConditional& c) :
IndexFactor(c) {
}
}

38
gtsam/linear/GaussianFactor.h
View File

@ -20,9 +20,7 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/linear/Errors.h>
#include <string>
#include <utility>
@ -75,8 +73,6 @@ namespace gtsam {
public:
enum SolveMethod { SOLVE_QR, SOLVE_PREFER_CHOLESKY };
typedef GaussianConditional ConditionalType;
typedef boost::shared_ptr<GaussianFactor> shared_ptr;
@ -96,40 +92,6 @@ namespace gtsam {
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) = 0;
/**
* Combine and eliminate several factors.
*/
static std::pair<boost::shared_ptr<BayesNet<GaussianConditional> >, shared_ptr> CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals=1, SolveMethod solveMethod=SOLVE_QR);
}; // GaussianFactor
/** unnormalized error */
template<class FACTOR>
double gaussianError(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) {
total_error += factor->error(x);
}
return total_error;
}
/** return A*x-b */
template<class FACTOR>
Errors gaussianErrors(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x);
}
/** shared pointer version */
template<class FACTOR>
boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) {
e->push_back(factor->error_vector(x));
}
return e;
}
} // namespace gtsam

560
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -14,126 +14,482 @@
* @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni
* @author Christian Potthast
* @author Richard Roberts
* @author Frank Dellaert
*/
#include <vector>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/HessianFactor.h>
using namespace std;
using namespace gtsam;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
INSTANTIATE_FACTOR_GRAPH(GaussianFactor)
;
// Explicitly instantiate so we don't have to include everywhere
INSTANTIATE_FACTOR_GRAPH(GaussianFactor);
/* ************************************************************************* */
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
FactorGraph<GaussianFactor> (CBN) {
}
/* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end());
return keys;
}
/* ************************************************************************* */
void GaussianFactorGraph::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_)
{
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) {
for (const_iterator factor = lfg.factors_.begin(); factor
!= lfg.factors_.end(); factor++) {
push_back(*factor);
}
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::combine2(
const GaussianFactorGraph& lfg1, const GaussianFactorGraph& lfg2) {
// create new linear factor graph equal to the first one
GaussianFactorGraph fg = lfg1;
// add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor
!= lfg2.factors_.end(); factor++) {
fg.push_back(*factor);
}
return fg;
}
/* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian(
const std::vector<size_t>& columnIndices) const {
std::vector<boost::tuple<size_t, size_t, double> > entries;
size_t i = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<
HessianFactor>(factor));
if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian));
else
throw invalid_argument(
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
// Add entries, adjusting the row index i
std::vector<boost::tuple<size_t, size_t, double> > factorEntries(
jacobianFactor->sparse(columnIndices));
entries.reserve(entries.size() + factorEntries.size());
for (size_t entry = 0; entry < factorEntries.size(); ++entry)
entries.push_back(boost::make_tuple(
factorEntries[entry].get<0> () + i, factorEntries[entry].get<
1> (), factorEntries[entry].get<2> ()));
// Increment row index
i += jacobianFactor->size1();
}
return entries;
}
// VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
// std::vector<size_t> dimensions(size()) ;
// Index i = 0 ;
// BOOST_FOREACH( const sharedFactor& factor, factors_) {
// dimensions[i] = factor->numberOfRows() ;
// i++;
// }
//
// return VectorValues(dimensions) ;
// }
//
// void GaussianFactorGraph::getb(VectorValues &b) const {
// Index i = 0 ;
// BOOST_FOREACH( const sharedFactor& factor, factors_) {
// b[i] = factor->getb();
// i++;
// }
// }
//
// VectorValues GaussianFactorGraph::getb() const {
// VectorValues b = allocateVectorValuesb() ;
// getb(b) ;
// return b ;
// }
/* ************************************************************************* */
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
FactorGraph<GaussianFactor> (CBN) {
}
// Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
#ifndef NDEBUG
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
vector<size_t> varDims(variableSlots.size());
#endif
size_t m = 0;
size_t n = 0;
{
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
/* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) keys.insert(factor->begin(), factor->end()); }
return keys;
}
assert(slots.second.size() == factors.size());
/* ************************************************************************* */
void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){
for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
push_back(*factor);
}
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1,
const GaussianFactorGraph& lfg2) {
// create new linear factor graph equal to the first one
GaussianFactorGraph fg = lfg1;
// add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor
!= lfg2.factors_.end(); factor++) {
fg.push_back(*factor);
}
return fg;
}
/* ************************************************************************* */
std::vector<boost::tuple<size_t,size_t,double> >
GaussianFactorGraph::sparseJacobian(const std::vector<size_t>& columnIndices) const {
std::vector<boost::tuple<size_t,size_t,double> > entries;
size_t i = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(!jacobianFactor) {
HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast<HessianFactor>(factor));
if(hessianFactor)
jacobianFactor.reset(new JacobianFactor(*hessianFactor));
else
throw invalid_argument("GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifndef NDEBUG
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
assert(varDims[jointVarpos] == vardim);
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
m += factor->size1();
}
// Add entries, adjusting the row index i
std::vector<boost::tuple<size_t,size_t,double> > factorEntries(jacobianFactor->sparse(columnIndices));
entries.reserve(entries.size() + factorEntries.size());
for(size_t entry=0; entry<factorEntries.size(); ++entry)
entries.push_back(boost::make_tuple(factorEntries[entry].get<0>()+i, factorEntries[entry].get<1>(), factorEntries[entry].get<2>()));
// Increment row index
i += jacobianFactor->size1();
}
return entries;
return boost::make_tuple(varDims, m, n);
}
// VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
// std::vector<size_t> dimensions(size()) ;
// Index i = 0 ;
// BOOST_FOREACH( const sharedFactor& factor, factors_) {
// dimensions[i] = factor->numberOfRows() ;
// i++;
// }
//
// return VectorValues(dimensions) ;
// }
//
// void GaussianFactorGraph::getb(VectorValues &b) const {
// Index i = 0 ;
// BOOST_FOREACH( const sharedFactor& factor, factors_) {
// b[i] = factor->getb();
// i++;
// }
// }
//
// VectorValues GaussianFactorGraph::getb() const {
// VectorValues b = allocateVectorValuesb() ;
// getb(b) ;
// return b ;
// }
/* ************************************************************************* */
JacobianFactor::shared_ptr CombineJacobians(
const FactorGraph<JacobianFactor>& factors,
const VariableSlots& variableSlots) {
const bool debug = ISDEBUG("CombineJacobians");
if (debug) factors.print("Combining factors: ");
if (debug) variableSlots.print();
if (debug) cout << "Determine dimensions" << endl;
tic(1, "countDims");
vector<size_t> varDims;
size_t m, n;
boost::tie(varDims, m, n) = countDims(factors, variableSlots);
if (debug) {
cout << "Dims: " << m << " x " << n << "\n";
BOOST_FOREACH(const size_t dim, varDims) cout << dim << " ";
cout << endl;
}
toc(1, "countDims");
if (debug) cout << "Sort rows" << endl;
tic(2, "sort rows");
vector<JacobianFactor::_RowSource> rowSources;
rowSources.reserve(m);
bool anyConstrained = false;
for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
const JacobianFactor& sourceFactor(*factors[sourceFactorI]);
sourceFactor.collectInfo(sourceFactorI, rowSources);
if (sourceFactor.isConstrained()) anyConstrained = true;
}
assert(rowSources.size() == m);
std::sort(rowSources.begin(), rowSources.end());
toc(2, "sort rows");
if (debug) cout << "Allocate new factor" << endl;
tic(3, "allocate");
JacobianFactor::shared_ptr combined(new JacobianFactor());
combined->allocate(variableSlots, varDims, m);
Vector sigmas(m);
toc(3, "allocate");
if (debug) cout << "Copy rows" << endl;
tic(4, "copy rows");
Index combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
for (size_t row = 0; row < m; ++row) {
const JacobianFactor::_RowSource& info(rowSources[row]);
const JacobianFactor& source(*factors[info.factorI]);
size_t sourceRow = info.factorRowI;
Index sourceSlot = varslot.second[info.factorI];
combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot);
}
++combinedSlot;
}
toc(4, "copy rows");
if (debug) cout << "Copy rhs (b), sigma, and firstNonzeroBlocks" << endl;
tic(5, "copy vectors");
for (size_t row = 0; row < m; ++row) {
const JacobianFactor::_RowSource& info(rowSources[row]);
const JacobianFactor& source(*factors[info.factorI]);
const size_t sourceRow = info.factorRowI;
combined->getb()(row) = source.getb()(sourceRow);
sigmas(row) = source.get_model()->sigmas()(sourceRow);
}
combined->copyFNZ(m, variableSlots.size(),rowSources);
toc(5, "copy vectors");
if (debug) cout << "Create noise model from sigmas" << endl;
tic(6, "noise model");
combined->setModel( anyConstrained,sigmas);
toc(6, "noise model");
if (debug) cout << "Assert Invariants" << endl;
combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
JacobianFactor>& factors, size_t nrFrontals) {
tic(1, "Combine");
JacobianFactor::shared_ptr jointFactor =
CombineJacobians(factors, VariableSlots(factors));
toc(1, "Combine");
tic(2, "eliminate");
GaussianBayesNet::shared_ptr gbn(jointFactor->eliminate(nrFrontals));
toc(2, "eliminate");
return make_pair(gbn, jointFactor);
}
/* ************************************************************************* */
static FastMap<Index, SlotEntry> findScatterAndDims//
(const FactorGraph<GaussianFactor>& factors) {
static const bool debug = false;
// The "scatter" is a map from global variable indices to slot indices in the
// union of involved variables. We also include the dimensionality of the
// variable.
Scatter scatter;
// First do the set union.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable))));
}
}
// Next fill in the slot indices (we can only get these after doing the set
// union.
size_t slot = 0;
BOOST_FOREACH(Scatter::value_type& var_slot, scatter) {
var_slot.second.slot = (slot ++);
if(debug)
cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl;
}
return scatter;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateCholesky");
// Find the scatter and variable dimensions
tic(1, "find scatter");
Scatter scatter(findScatterAndDims(factors));
toc(1, "find scatter");
// Pull out keys and dimensions
tic(2, "keys");
vector<Index> keys(scatter.size());
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
keys[var_slot.second.slot] = var_slot.first;
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
toc(2, "keys");
// Form Ab' * Ab
tic(3, "combine");
HessianFactor::shared_ptr //
combinedFactor(new HessianFactor(factors, dimensions, scatter));
toc(3, "combine");
// Do Cholesky, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
tic(4, "partial Cholesky");
combinedFactor->partialCholesky(nrFrontals);
toc(4, "partial Cholesky");
// Extract conditionals and fill in details of the remaining factor
tic(5, "split");
GaussianBayesNet::shared_ptr conditionals =
combinedFactor->splitEliminatedFactor(nrFrontals, keys);
if (debug) {
conditionals->print("Extracted conditionals: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
toc(5, "split");
combinedFactor->assertInvariants();
return make_pair(conditionals, combinedFactor);
}
/* ************************************************************************* */
static FactorGraph<JacobianFactor> convertToJacobians(const FactorGraph<
GaussianFactor>& factors) {
typedef JacobianFactor J;
typedef HessianFactor H;
const bool debug = ISDEBUG("convertToJacobians");
FactorGraph<J> jacobians;
jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
if (factor) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian) {
jacobians.push_back(jacobian);
if (debug) jacobian->print("Existing JacobianFactor: ");
} else {
H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
if (!hessian) throw std::invalid_argument(
"convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor.");
J::shared_ptr converted(new J(*hessian));
if (debug) {
if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error(
"convertToJacobians: Conversion between Jacobian and Hessian incorrect");
cout << "Converted HessianFactor to JacobianFactor:\n";
hessian->print("HessianFactor: ");
converted->print("JacobianFactor: ");
}
jacobians.push_back(converted);
}
}
return jacobians;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateQR");
// Convert all factors to the appropriate type and call the type-specific EliminateGaussian.
if (debug) cout << "Using QR:";
tic(1, "convert to Jacobian");
FactorGraph<JacobianFactor> jacobians = convertToJacobians(factors);
toc(1, "convert to Jacobian");
tic(2, "Jacobian EliminateGaussian");
GaussianBayesNet::shared_ptr bn;
GaussianFactor::shared_ptr factor;
boost::tie(bn, factor) = EliminateJacobians(jacobians, nrFrontals);
toc(2, "Jacobian EliminateGaussian");
return make_pair(bn, factor);
} // EliminateQR
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
typedef JacobianFactor J;
typedef HessianFactor H;
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but Cholesky cannot.
// Decide whether to use QR or Cholesky
// Check if any JacobianFactors have constrained noise models.
bool useQR = false;
useQR = false;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
useQR = true;
break;
}
}
// Convert all factors to the appropriate type
// and call the type-specific EliminateGaussian.
if (useQR) return EliminateQR(factors, nrFrontals);
GaussianFactorGraph::EliminationResult ret;
try {
tic(2, "EliminateCholesky");
ret = EliminateCholesky(factors, nrFrontals);
toc(2, "EliminateCholesky");
} catch (const exception& e) {
cout << "Exception in EliminateCholesky: " << e.what() << endl;
SETDEBUG("EliminateCholesky", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("choleskyPartial", true);
factors.print("Combining factors: ");
EliminateCholesky(factors, nrFrontals);
throw;
}
const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky");
if (checkCholesky) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
expected_factor, actual_factor, 1.0)) {
cout << "Cholesky and QR do not agree" << endl;
SETDEBUG("EliminateCholesky", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateCholesky(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("Cholesky and QR do not agree");
}
}
return ret;
} // EliminatePreferCholesky
} // namespace gtsam

55
gtsam/linear/GaussianFactorGraph.h
View File

@ -15,6 +15,8 @@
* @author Kai Ni
* @author Christian Potthast
* @author Alireza Fathi
* @author Richard Roberts
* @author Frank Dellaert
*/
#pragma once
@ -25,13 +27,40 @@
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/SharedDiagonal.h>
namespace gtsam {
class SharedDiagonal;
/** unnormalized error */
template<class FACTOR>
double gaussianError(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) {
total_error += factor->error(x);
}
return total_error;
}
/** return A*x-b */
template<class FACTOR>
Errors gaussianErrors(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x);
}
/** shared pointer version */
template<class FACTOR>
boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) {
e->push_back(factor->error_vector(x));
}
return e;
}
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor
@ -138,4 +167,26 @@ namespace gtsam {
};
/**
* Combine and eliminate several factors.
*/
JacobianFactor::shared_ptr CombineJacobians(
const FactorGraph<JacobianFactor>& factors,
const VariableSlots& variableSlots);
GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
JacobianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateHessians(const FactorGraph<
HessianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
} // namespace gtsam

31
gtsam/linear/GaussianISAM.cpp
View File

@ -28,14 +28,27 @@ namespace ublas = boost::numeric::ublas;
namespace gtsam {
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianISAM::marginalFactor(Index j) const {
return Super::marginalFactor(j, &EliminateQR);
}
/* ************************************************************************* */
BayesNet<GaussianConditional>::shared_ptr GaussianISAM::marginalBayesNet(Index j) const {
return Super::marginalBayesNet(j, &EliminateQR);
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianISAM::marginal(Index j) const {
GaussianFactor::shared_ptr factor = this->marginalFactor(j);
FactorGraph<GaussianFactor> graph;
graph.push_back(factor);
GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(graph,1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
}
/* ************************************************************************* */
BayesNet<GaussianConditional>::shared_ptr GaussianISAM::jointBayesNet(
Index key1, Index key2) const {
return Super::jointBayesNet(key1, key2, &EliminateQR);
}
/* ************************************************************************* */
@ -60,4 +73,10 @@ VectorValues optimize(const GaussianISAM& bayesTree) {
return result;
}
/* ************************************************************************* */
BayesNet<GaussianConditional> GaussianISAM::shortcut(sharedClique clique, sharedClique root) {
return clique->shortcut(root,&EliminateQR);
}
/* ************************************************************************* */
} /// namespace gtsam

22
gtsam/linear/GaussianISAM.h
View File

@ -19,29 +19,30 @@
#pragma once
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam {
class GaussianISAM : public ISAM<GaussianConditional> {
typedef ISAM<GaussianConditional> Super;
std::deque<size_t, boost::fast_pool_allocator<size_t> > dims_;
public:
/** Create an empty Bayes Tree */
GaussianISAM() : ISAM<GaussianConditional>() {}
GaussianISAM() : Super() {}
/** Create a Bayes Tree from a Bayes Net */
GaussianISAM(const GaussianBayesNet& bayesNet) : ISAM<GaussianConditional>(bayesNet) {}
GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {}
/** Override update_internal to also keep track of variable dimensions. */
template<class FACTORGRAPH>
void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
ISAM<GaussianConditional>::update_internal(newFactors, orphans);
Super::update_internal(newFactors, orphans, &EliminateQR);
// update dimensions
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) {
@ -63,15 +64,22 @@ public:
}
void clear() {
ISAM<GaussianConditional>::clear();
Super::clear();
dims_.clear();
}
friend VectorValues optimize(const GaussianISAM&);
/** return marginal on any variable */
/** return marginal on any variable as a factor, Bayes net, or mean/cov */
GaussianFactor::shared_ptr marginalFactor(Index j) const;
BayesNet<GaussianConditional>::shared_ptr marginalBayesNet(Index key) const;
std::pair<Vector,Matrix> marginal(Index key) const;
/** return joint between two variables, as a Bayes net */
BayesNet<GaussianConditional>::shared_ptr jointBayesNet(Index key1, Index key2) const;
/** return the conditional P(S|Root) on the separator given the root */
static BayesNet<GaussianConditional> shortcut(sharedClique clique, sharedClique root);
};
// recursively optimize this conditional and all subtrees

8
gtsam/linear/GaussianJunctionTree.cpp
View File

@ -34,9 +34,6 @@ namespace gtsam {
using namespace std;
/* ************************************************************************* */
/**
* GaussianJunctionTree
*/
void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const {
// solve the bayes net in the current node
GaussianBayesNet::const_reverse_iterator it = current->rbegin();
@ -64,10 +61,10 @@ namespace gtsam {
}
/* ************************************************************************* */
VectorValues GaussianJunctionTree::optimize() const {
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());
boost::shared_ptr<const BayesTree::Clique> rootClique(this->eliminate(function));
toc(1, "GJT eliminate");
// Allocate solution vector
@ -84,4 +81,5 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
} //namespace gtsam

5
gtsam/linear/GaussianJunctionTree.h
View File

@ -32,10 +32,12 @@ namespace gtsam {
* GaussianJunctionTree that does the optimization
*/
class GaussianJunctionTree: public JunctionTree<GaussianFactorGraph> {
public:
typedef boost::shared_ptr<GaussianJunctionTree> shared_ptr;
typedef JunctionTree<GaussianFactorGraph> Base;
typedef Base::sharedClique sharedClique;
typedef GaussianFactorGraph::Eliminate Eliminate;
protected:
// back-substitute in topological sort order (parents first)
@ -53,7 +55,8 @@ namespace gtsam {
GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex) : Base(fg, variableIndex) {}
// optimize the linear graph
VectorValues optimize() const;
VectorValues optimize(Eliminate function) const;
}; // GaussianJunctionTree
} // namespace gtsam

12
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -48,32 +48,32 @@ void GaussianMultifrontalSolver::replaceFactors(const FactorGraph<GaussianFactor
/* ************************************************************************* */
BayesTree<GaussianConditional>::shared_ptr GaussianMultifrontalSolver::eliminate() const {
return Base::eliminate();
return Base::eliminate(&EliminateQR);
}
/* ************************************************************************* */
VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
tic(2,"optimize");
VectorValues::shared_ptr values(new VectorValues(junctionTree_->optimize()));
VectorValues::shared_ptr values(new VectorValues(junctionTree_->optimize(&EliminateQR)));
toc(2,"optimize");
return values;
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js)));
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR)));
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const {
return Base::marginalFactor(j);
return Base::marginalFactor(j,&EliminateQR);
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalCovariance(Index j) const {
FactorGraph<GaussianFactor> fg;
fg.push_back(Base::marginalFactor(j));
GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(fg,1).first->front();
fg.push_back(Base::marginalFactor(j,&EliminateQR));
GaussianConditional::shared_ptr conditional = EliminateQR(fg,1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
}

27
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -49,7 +49,7 @@ void GaussianSequentialSolver::replaceFactors(const FactorGraph<GaussianFactor>:
/* ************************************************************************* */
GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const {
return Base::eliminate();
return Base::eliminate(&EliminateQR);
}
/* ************************************************************************* */
@ -82,20 +82,23 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const {
return Base::marginalFactor(j);
}
std::pair<Vector, Matrix> GaussianSequentialSolver::marginalCovariance(Index j) const {
FactorGraph<GaussianFactor> fg;
fg.push_back(Base::marginalFactor(j));
GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(fg,1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
return Base::marginalFactor(j,&EliminateQR);
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianSequentialSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js)));
std::pair<Vector, Matrix> GaussianSequentialSolver::marginalCovariance(Index j) const {
FactorGraph<GaussianFactor> fg;
fg.push_back(Base::marginalFactor(j, &EliminateQR));
GaussianConditional::shared_ptr conditional = EliminateQR(fg, 1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R),R)));
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr
GaussianSequentialSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(
*Base::jointFactorGraph(js, &EliminateQR)));
}
}

146
gtsam/linear/HessianFactor.cpp
View File

@ -173,6 +173,40 @@ namespace gtsam {
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
const vector<size_t>& dimensions, const Scatter& scatter) :
info_(matrix_) {
const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab
tic(1, "allocate");
info_.resize(dimensions.begin(), dimensions.end(), false);
toc(1, "allocate");
tic(2, "zero");
ublas::noalias(matrix_) = ublas::zero_matrix<double>(matrix_.size1(),matrix_.size2());
toc(2, "zero");
tic(3, "update");
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
updateATA(*hessian, scatter);
else {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (jacobianFactor)
updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
}
}
toc(3, "update");
if (debug) gtsam::print(matrix_, "Ab' * Ab: ");
assertInvariants();
}
/* ************************************************************************* */
void HessianFactor::print(const std::string& s) const {
cout << s << "\n";
cout << " keys: ";
@ -202,36 +236,6 @@ namespace gtsam {
2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0));
}
/* ************************************************************************* */
static FastMap<Index, SlotEntry> findScatterAndDims(const FactorGraph<GaussianFactor>& factors) {
static const bool debug = false;
// The "scatter" is a map from global variable indices to slot indices in the
// union of involved variables. We also include the dimensionality of the
// variable.
Scatter scatter;
// First do the set union.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable))));
}
}
// Next fill in the slot indices (we can only get these after doing the set
// union.
size_t slot = 0;
BOOST_FOREACH(Scatter::value_type& var_slot, scatter) {
var_slot.second.slot = (slot ++);
if(debug)
cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl;
}
return scatter;
}
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) {
// This function updates 'combined' with the information in 'update'.
@ -433,7 +437,14 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
// toc(2, "update");
}
GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& keys) {
/* ************************************************************************* */
void HessianFactor::partialCholesky(size_t nrFrontals) {
choleskyPartial(matrix_, info_.offset(nrFrontals));
}
/* ************************************************************************* */
GaussianBayesNet::shared_ptr
HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& keys) {
static const bool debug = false;
@ -482,75 +493,4 @@ GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFront
return conditionals;
}
/* ************************************************************************* */
pair<GaussianBayesNet::shared_ptr, HessianFactor::shared_ptr> HessianFactor::CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("HessianFactor::CombineAndEliminate");
// Find the scatter and variable dimensions
tic(1, "find scatter");
Scatter scatter(findScatterAndDims(factors));
toc(1, "find scatter");
// Pull out keys and dimensions
tic(2, "keys");
vector<Index> keys(scatter.size());
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
keys[var_slot.second.slot] = var_slot.first;
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
toc(2, "keys");
// Form Ab' * Ab
tic(3, "combine");
tic(1, "allocate");
HessianFactor::shared_ptr combinedFactor(new HessianFactor());
combinedFactor->info_.resize(dimensions.begin(), dimensions.end(), false);
toc(1, "allocate");
tic(2, "zero");
ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix<double>(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2());
toc(2, "zero");
tic(3, "update");
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast<HessianFactor>(factor));
if(hessianFactor)
combinedFactor->updateATA(*hessianFactor, scatter);
else {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(jacobianFactor)
combinedFactor->updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
}
}
toc(3, "update");
if(debug) gtsam::print(combinedFactor->matrix_, "Ab' * Ab: ");
toc(3, "combine");
// Do Cholesky, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
tic(4, "partial Cholesky");
choleskyPartial(combinedFactor->matrix_, combinedFactor->info_.offset(nrFrontals));
if(debug) gtsam::print(combinedFactor->matrix_, "chol(Ab' * Ab): ");
toc(4, "partial Cholesky");
// Extract conditionals and fill in details of the remaining factor
tic(5, "split");
GaussianBayesNet::shared_ptr conditionals(combinedFactor->splitEliminatedFactor(nrFrontals, keys));
if(debug) {
conditionals->print("Extracted conditionals: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
toc(5, "split");
combinedFactor->assertInvariants();
return make_pair(conditionals, combinedFactor);
}
}
} // gtsam

33
gtsam/linear/HessianFactor.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
// Forward declarations for friend unit tests
@ -49,8 +50,6 @@ namespace gtsam {
InfoMatrix matrix_; // The full information matrix [A b]^T * [A b]
BlockInfo info_; // The block view of the full information matrix.
void assertInvariants() const;
boost::shared_ptr<BayesNet<GaussianConditional> > splitEliminatedFactor(size_t nrFrontals, const std::vector<Index>& keys);
void updateATA(const JacobianFactor& update, const Scatter& scatter);
void updateATA(const HessianFactor& update, const Scatter& scatter);
@ -96,7 +95,12 @@ namespace gtsam {
/** Convert from a JacobianFactor or HessianFactor (computes A^T * A) */
HessianFactor(const GaussianFactor& factor);
virtual ~HessianFactor() {}
/** Special constructor used in EliminateCholesky */
HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter);
virtual ~HessianFactor() {
}
// Implementing Testable interface
virtual void print(const std::string& s = "") const;
@ -121,13 +125,7 @@ namespace gtsam {
* variable. The order of the variables within the factor is not changed.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) {
Factor<Index>::permuteWithInverse(inversePermutation); }
/**
* Combine and eliminate several factors.
*/
static std::pair<boost::shared_ptr<BayesNet<GaussianConditional> >, shared_ptr> CombineAndEliminate(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals=1);
IndexFactor::permuteWithInverse(inversePermutation); }
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
@ -135,6 +133,21 @@ namespace gtsam {
// Friend JacobianFactor for conversion
friend class JacobianFactor;
// used in eliminateCholesky:
/**
* Do Cholesky. Note that after this, the lower triangle still contains
* some untouched non-zeros that should be zero. We zero them while
* extracting submatrices in splitEliminatedFactor. Frank says :-(
*/
void partialCholesky(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<BayesNet<GaussianConditional> > splitEliminatedFactor(
size_t nrFrontals, const std::vector<Index>& keys);
/** assert invariants */
void assertInvariants() const;
};
}

217
gtsam/linear/JacobianFactor.cpp
View File

@ -493,175 +493,68 @@ namespace gtsam {
}
/* ************************************************************************* */
/* Used internally by JacobianFactor::Combine for sorting */
struct _RowSource {
size_t firstNonzeroVar;
size_t factorI;
size_t factorRowI;
_RowSource(size_t _firstNonzeroVar, size_t _factorI, size_t _factorRowI) :
firstNonzeroVar(_firstNonzeroVar), factorI(_factorI), factorRowI(_factorRowI) {}
bool operator<(const _RowSource& o) const { return firstNonzeroVar < o.firstNonzeroVar; }
};
void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const {
assertInvariants();
for (size_t row = 0; row < size1(); ++row) {
Index firstNonzeroVar;
if (firstNonzeroBlocks_[row] < size()) {
firstNonzeroVar = keys_[firstNonzeroBlocks_[row]];
} else if (firstNonzeroBlocks_[row] == size()) {
firstNonzeroVar = back() + 1;
} else {
assert(false);
}
rowSources.push_back(_RowSource(firstNonzeroVar, index, row));
}
}
/* ************************************************************************* */
// Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
#ifndef NDEBUG
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
vector<size_t> varDims(variableSlots.size());
#endif
size_t m = 0;
size_t n = 0;
{
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size());
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifndef NDEBUG
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
assert(varDims[jointVarpos] == vardim);
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
m += factor->size1();
}
}
return boost::make_tuple(varDims, m, n);
}
void JacobianFactor::allocate(const VariableSlots& variableSlots, vector<
size_t>& varDims, size_t m) {
keys_.resize(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), keys_.begin(),
bind(&VariableSlots::const_iterator::value_type::first,
boost::lambda::_1));
varDims.push_back(1);
Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));
firstNonzeroBlocks_.resize(m);
}
/* ************************************************************************* */
JacobianFactor::shared_ptr JacobianFactor::Combine(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
const bool debug = ISDEBUG("JacobianFactor::Combine");
if(debug) factors.print("Combining factors: ");
if(debug) variableSlots.print();
// Determine dimensions
tic(1, "countDims");
vector<size_t> varDims;
size_t m;
size_t n;
boost::tie(varDims, m, n) = countDims(factors, variableSlots);
if(debug) {
cout << "Dims: " << m << " x " << n << "\n";
BOOST_FOREACH(const size_t dim, varDims) { cout << dim << " "; }
cout << endl;
}
toc(1, "countDims");
// Sort rows
tic(2, "sort rows");
vector<_RowSource> rowSources; rowSources.reserve(m);
bool anyConstrained = false;
for(size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
const JacobianFactor& sourceFactor(*factors[sourceFactorI]);
sourceFactor.assertInvariants();
for(size_t sourceFactorRow = 0; sourceFactorRow < sourceFactor.size1(); ++sourceFactorRow) {
Index firstNonzeroVar;
if(sourceFactor.firstNonzeroBlocks_[sourceFactorRow] < sourceFactor.size())
firstNonzeroVar = sourceFactor.keys_[sourceFactor.firstNonzeroBlocks_[sourceFactorRow]];
else if(sourceFactor.firstNonzeroBlocks_[sourceFactorRow] == sourceFactor.size())
firstNonzeroVar = sourceFactor.back() + 1;
else
assert(false);
rowSources.push_back(_RowSource(firstNonzeroVar, sourceFactorI, sourceFactorRow));
}
if(sourceFactor.model_->isConstrained()) anyConstrained = true;
}
assert(rowSources.size() == m);
std::sort(rowSources.begin(), rowSources.end());
toc(2, "sort rows");
// Allocate new factor
tic(3, "allocate");
shared_ptr combined(new JacobianFactor());
combined->keys_.resize(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), combined->keys_.begin(), bind(&VariableSlots::const_iterator::value_type::first, boost::lambda::_1));
varDims.push_back(1);
combined->Ab_.copyStructureFrom(BlockAb(combined->matrix_, varDims.begin(), varDims.end(), m));
combined->firstNonzeroBlocks_.resize(m);
Vector sigmas(m);
toc(3, "allocate");
// Copy rows
tic(4, "copy rows");
Index combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
for(size_t row = 0; row < m; ++row) {
const Index sourceSlot = varslot.second[rowSources[row].factorI];
ABlock combinedBlock(combined->Ab_(combinedSlot));
if(sourceSlot != numeric_limits<Index>::max()) {
const JacobianFactor& source(*factors[rowSources[row].factorI]);
const size_t sourceRow = rowSources[row].factorRowI;
if(source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
const constABlock sourceBlock(source.Ab_(sourceSlot));
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow);
} else
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<double>(combinedBlock.size2());
} else
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<double>(combinedBlock.size2());
}
++ combinedSlot;
}
toc(4, "copy rows");
// Copy rhs (b), sigma, and firstNonzeroBlocks
tic(5, "copy vectors");
Index firstNonzeroSlot = 0;
for(size_t row = 0; row < m; ++row) {
const JacobianFactor& source(*factors[rowSources[row].factorI]);
const size_t sourceRow = rowSources[row].factorRowI;
combined->getb()(row) = source.getb()(sourceRow);
sigmas(row) = source.get_model()->sigmas()(sourceRow);
while(firstNonzeroSlot < variableSlots.size() && rowSources[row].firstNonzeroVar > combined->keys_[firstNonzeroSlot])
++ firstNonzeroSlot;
combined->firstNonzeroBlocks_[row] = firstNonzeroSlot;
}
toc(5, "copy vectors");
// Create noise model from sigmas
tic(6, "noise model");
if(anyConstrained) combined->model_ = noiseModel::Constrained::MixedSigmas(sigmas);
else combined->model_ = noiseModel::Diagonal::Sigmas(sigmas);
toc(6, "noise model");
combined->assertInvariants();
return combined;
}
void JacobianFactor::copyRow(const JacobianFactor& source, Index sourceRow,
size_t sourceSlot, size_t row, Index slot) {
ABlock combinedBlock(Ab_(slot));
if (sourceSlot != numeric_limits<Index>::max()) {
if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
const constABlock sourceBlock(source.Ab_(sourceSlot));
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(
sourceBlock, sourceRow);
} else
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<
double>(combinedBlock.size2());
} else
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<
double>(combinedBlock.size2());
}
/* ************************************************************************* */
pair<GaussianBayesNet::shared_ptr, JacobianFactor::shared_ptr> JacobianFactor::CombineAndEliminate(
const FactorGraph<JacobianFactor>& factors, size_t nrFrontals) {
tic(1, "Combine");
shared_ptr jointFactor(Combine(factors, VariableSlots(factors)));
toc(1, "Combine");
tic(2, "eliminate");
GaussianBayesNet::shared_ptr gbn(jointFactor->eliminate(nrFrontals));
toc(2, "eliminate");
return make_pair(gbn, jointFactor);
}
void JacobianFactor::copyFNZ(size_t m, size_t n,
vector<_RowSource>& rowSources) {
Index i = 0;
for (size_t row = 0; row < m; ++row) {
while (i < n && rowSources[row].firstNonzeroVar > keys_[i])
++i;
firstNonzeroBlocks_[row] = i;
}
}
/* ************************************************************************* */
void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
if (anyConstrained)
model_ = noiseModel::Constrained::MixedSigmas(sigmas);
else
model_ = noiseModel::Diagonal::Sigmas(sigmas);
}
/* ************************************************************************* */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {

53
gtsam/linear/JacobianFactor.h
View File

@ -21,6 +21,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/Errors.h>
@ -58,10 +59,6 @@ namespace gtsam {
typedef BlockAb::Column BVector;
typedef BlockAb::constColumn constBVector;
protected:
void assertInvariants() const;
static JacobianFactor::shared_ptr Combine(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots);
public:
/** Copy constructor */
@ -116,6 +113,9 @@ namespace gtsam {
*/
bool empty() const { return Ab_.size1() == 0;}
/** is noise model constrained ? */
bool isConstrained() const { return model_->isConstrained();}
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
*/
@ -194,12 +194,6 @@ namespace gtsam {
* model. */
JacobianFactor whiten() const;
/**
* Combine and eliminate several factors.
*/
static std::pair<boost::shared_ptr<BayesNet<GaussianConditional> >, shared_ptr> CombineAndEliminate(
const FactorGraph<JacobianFactor>& factors, size_t nrFrontals=1);
boost::shared_ptr<GaussianConditional> eliminateFirst();
boost::shared_ptr<BayesNet<GaussianConditional> > eliminate(size_t nrFrontals = 1);
@ -210,8 +204,43 @@ namespace gtsam {
// Friend unit tests (see also forward declarations above)
friend class ::Combine2GaussianFactorTest;
friend class ::eliminateFrontalsGaussianFactorTest;
};
/* Used by ::CombineJacobians for sorting */
struct _RowSource {
size_t firstNonzeroVar;
size_t factorI;
size_t factorRowI;
_RowSource(size_t _firstNonzeroVar, size_t _factorI, size_t _factorRowI) :
firstNonzeroVar(_firstNonzeroVar), factorI(_factorI), factorRowI(_factorRowI) {}
bool operator<(const _RowSource& o) const {
return firstNonzeroVar < o.firstNonzeroVar;
}
};
// following methods all used in CombineJacobians:
// Many imperative, perhaps all need to be combined in constructor
/** Collect information on Jacobian rows */
void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const;
/** allocate space */
void allocate(const VariableSlots& variableSlots,
std::vector<size_t>& varDims, size_t m);
/** copy a slot from source */
void copyRow(const JacobianFactor& source,
Index sourceRow, size_t sourceSlot, size_t row, Index slot);
/** copy firstNonzeroBlocks structure */
void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources);
/** set noiseModel correctly */
void setModel(bool anyConstrained, const Vector& sigmas);
/** Assert invariants after elimination */
void assertInvariants() const;
}; // JacobianFactor
/** return A*x */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x);
@ -237,5 +266,5 @@ namespace gtsam {
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x);
}
} // gtsam

2
gtsam/linear/Makefile.am
View File

@ -31,7 +31,7 @@ sources += GaussianJunctionTree.cpp
sources += GaussianConditional.cpp GaussianBayesNet.cpp
sources += GaussianISAM.cpp
check_PROGRAMS += tests/testHessianFactor tests/testGaussianFactor tests/testGaussianConditional
check_PROGRAMS += tests/testGaussianJunctionTree
check_PROGRAMS += tests/testGaussianFactorGraph tests/testGaussianJunctionTree
# Iterative Methods
headers += iterative-inl.h

1
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -18,6 +18,7 @@
#include <boost/foreach.hpp>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/FactorGraph-inl.h>
using namespace std;

3
gtsam/linear/SubgraphSolver-inl.h
View File

@ -80,7 +80,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
// theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedBayesNet Rc1 =
EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate(&EliminateQR);
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
pc_ = boost::make_shared<SubgraphPreconditioner>(

661
gtsam/linear/tests/testGaussianFactor.cpp
View File

@ -16,32 +16,14 @@
* @author Frank Dellaert
**/
#include <iostream>
#include <vector>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/map.hpp> // for insert
using namespace boost::assign;
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
//#define GTSAM_MAGIC_KEY
#include <gtsam/base/types.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/GaussianConditional.h>
using namespace std;
using namespace gtsam;
using namespace boost;
namespace ublas = boost::numeric::ublas;
static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
@ -87,302 +69,37 @@ TEST(GaussianFactor, constructor2)
EXPECT(assert_equal(b, actualb));
}
///* ************************************************************************* */
//TEST(GaussianFactor, Combine)
//{
// Matrix A00 = Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0);
// Vector b0 = Vector_(3, 0.0, 0.0, 0.0);
// Vector s0 = Vector_(3, 0.0, 0.0, 0.0);
//
// Matrix A10 = Matrix_(3,3,
// 0.0, -2.0, -4.0,
// 2.0, 0.0, 2.0,
// 0.0, 0.0, -10.0);
// Matrix A11 = Matrix_(3,3,
// 2.0, 0.0, 0.0,
// 0.0, 2.0, 0.0,
// 0.0, 0.0, 10.0);
// Vector b1 = Vector_(3, 6.0, 2.0, 0.0);
// Vector s1 = Vector_(3, 1.0, 1.0, 1.0);
//
// Matrix A20 = Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0);
// Vector b2 = Vector_(3, 0.0, 0.0, 0.0);
// Vector s2 = Vector_(3, 100.0, 100.0, 100.0);
//
// GaussianFactorGraph gfg;
// gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true));
// gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
// gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true));
//
// GaussianVariableIndex varindex(gfg);
// vector<size_t> factors(3); factors[0]=0; factors[1]=1; factors[2]=2;
// vector<size_t> variables(2); variables[0]=0; variables[1]=1;
// vector<vector<size_t> > variablePositions(3);
// variablePositions[0].resize(1); variablePositions[0][0]=0;
// variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1;
// variablePositions[2].resize(1); variablePositions[2][0]=0;
//
// JacobianFactor actual = *JacobianFactor::Combine(gfg, varindex, factors, variables, variablePositions);
//
// Matrix zero3x3 = zeros(3,3);
// Matrix A0 = gtsam::stack(3, &A00, &A10, &A20);
// Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3);
// Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
// Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
//
// JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
//
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */
TEST(GaussianFactor, Combine2)
#ifdef BROKEN
TEST(GaussianFactor, operators )
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix I = eye(2);
Vector b = Vector_(2,0.2,-0.1);
JacobianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
VectorValues c;
c[_x1_] = Vector_(2,10.,20.);
c[_x2_] = Vector_(2,30.,60.);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
// test A*x
Vector expectedE = Vector_(2,200.,400.), e = lf*c;
EXPECT(assert_equal(expectedE,e));
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
// test A^e
VectorValues expectedX;
expectedX[_x1_] = Vector_(2,-2000.,-4000.);
expectedX[_x2_] = Vector_(2, 2000., 4000.);
EXPECT(assert_equal(expectedX,lf^e));
JacobianFactor actual = *JacobianFactor::Combine(*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> >(), VariableSlots(gfg));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
EXPECT(assert_equal(expected, actual));
// test transposeMultiplyAdd
VectorValues x;
x[_x1_] = Vector_(2, 1.,2.);
x[_x2_] = Vector_(2, 3.,4.);
VectorValues expectedX2 = x + 0.1 * (lf^e);
lf.transposeMultiplyAdd(0.1,e,x);
EXPECT(assert_equal(expectedX2,x));
}
/* ************************************************************************* */
TEST_UNSAFE(GaussianFactor, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianBayesNet expectedBN(*expectedFactor.eliminate(1));
pair<GaussianBayesNet::shared_ptr, JacobianFactor::shared_ptr> actualQR(JacobianFactor::CombineAndEliminate(
*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> >(), 1));
EXPECT(assert_equal(expectedBN, *actualQR.first));
EXPECT(assert_equal(expectedFactor, *actualQR.second));
}
///* ************************************************************************* */
//TEST(GaussianFactor, operators )
//{
// Matrix I = eye(2);
// Vector b = Vector_(2,0.2,-0.1);
// JacobianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1);
//
// VectorValues c;
// c.insert(_x1_,Vector_(2,10.,20.));
// c.insert(_x2_,Vector_(2,30.,60.));
//
// // test A*x
// Vector expectedE = Vector_(2,200.,400.), e = lf*c;
// EXPECT(assert_equal(expectedE,e));
//
// // test A^e
// VectorValues expectedX;
// expectedX.insert(_x1_,Vector_(2,-2000.,-4000.));
// expectedX.insert(_x2_,Vector_(2, 2000., 4000.));
// EXPECT(assert_equal(expectedX,lf^e));
//
// // test transposeMultiplyAdd
// VectorValues x;
// x.insert(_x1_,Vector_(2, 1.,2.));
// x.insert(_x2_,Vector_(2, 3.,4.));
// VectorValues expectedX2 = x + 0.1 * (lf^e);
// lf.transposeMultiplyAdd(0.1,e,x);
// EXPECT(assert_equal(expectedX2,x));
//}
///* ************************************************************************* */
//TEST( NonlinearFactorGraph, combine2){
// double sigma1 = 0.0957;
// Matrix A11(2,2);
// A11(0,0) = 1; A11(0,1) = 0;
// A11(1,0) = 0; A11(1,1) = 1;
// Vector b(2);
// b(0) = 2; b(1) = -1;
// JacobianFactor::shared_ptr f1(new JacobianFactor(_x1_, A11, b*sigma1, sharedSigma(2,sigma1)));
//
// double sigma2 = 0.5;
// A11(0,0) = 1; A11(0,1) = 0;
// A11(1,0) = 0; A11(1,1) = -1;
// b(0) = 4 ; b(1) = -5;
// JacobianFactor::shared_ptr f2(new JacobianFactor(_x1_, A11, b*sigma2, sharedSigma(2,sigma2)));
//
// double sigma3 = 0.25;
// A11(0,0) = 1; A11(0,1) = 0;
// A11(1,0) = 0; A11(1,1) = -1;
// b(0) = 3 ; b(1) = -88;
// JacobianFactor::shared_ptr f3(new JacobianFactor(_x1_, A11, b*sigma3, sharedSigma(2,sigma3)));
//
// // TODO: find a real sigma value for this example
// double sigma4 = 0.1;
// A11(0,0) = 6; A11(0,1) = 0;
// A11(1,0) = 0; A11(1,1) = 7;
// b(0) = 5 ; b(1) = -6;
// JacobianFactor::shared_ptr f4(new JacobianFactor(_x1_, A11*sigma4, b*sigma4, sharedSigma(2,sigma4)));
//
// vector<JacobianFactor::shared_ptr> lfg;
// lfg.push_back(f1);
// lfg.push_back(f2);
// lfg.push_back(f3);
// lfg.push_back(f4);
// JacobianFactor combined(lfg);
//
// Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4);
// Matrix A22(8,2);
// A22(0,0) = 1; A22(0,1) = 0;
// A22(1,0) = 0; A22(1,1) = 1;
// A22(2,0) = 1; A22(2,1) = 0;
// A22(3,0) = 0; A22(3,1) = -1;
// A22(4,0) = 1; A22(4,1) = 0;
// A22(5,0) = 0; A22(5,1) = -1;
// A22(6,0) = 0.6; A22(6,1) = 0;
// A22(7,0) = 0; A22(7,1) = 0.7;
// Vector exb(8);
// exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2;
// exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4;
//
// vector<pair<Index, Matrix> > meas;
// meas.push_back(make_pair(_x1_, A22));
// JacobianFactor expected(meas, exb, sigmas);
// EXPECT(assert_equal(expected,combined));
//}
//
///* ************************************************************************* */
//TEST(GaussianFactor, linearFactorN){
// Matrix I = eye(2);
// vector<JacobianFactor::shared_ptr> f;
// SharedDiagonal model = sharedSigma(2,1.0);
// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, I, Vector_(2,
// 10.0, 5.0), model)));
// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, -10 * I,
// _x2_, 10 * I, Vector_(2, 1.0, -2.0), model)));
// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x2_, -10 * I,
// _x3_, 10 * I, Vector_(2, 1.5, -1.5), model)));
// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x3_, -10 * I,
// _x4_, 10 * I, Vector_(2, 2.0, -1.0), model)));
//
// JacobianFactor combinedFactor(f);
//
// vector<pair<Index, Matrix> > combinedMeasurement;
// combinedMeasurement.push_back(make_pair(_x1_, Matrix_(8,2,
// 1.0, 0.0,
// 0.0, 1.0,
// -10.0, 0.0,
// 0.0,-10.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0)));
// combinedMeasurement.push_back(make_pair(_x2_, Matrix_(8,2,
// 0.0, 0.0,
// 0.0, 0.0,
// 10.0, 0.0,
// 0.0, 10.0,
// -10.0, 0.0,
// 0.0,-10.0,
// 0.0, 0.0,
// 0.0, 0.0)));
// combinedMeasurement.push_back(make_pair(_x3_, Matrix_(8,2,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 10.0, 0.0,
// 0.0, 10.0,
// -10.0, 0.0,
// 0.0,-10.0)));
// combinedMeasurement.push_back(make_pair(_x4_, Matrix_(8,2,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 0.0, 0.0,
// 10.0, 0.0,
// 0.0,10.0)));
// Vector b = Vector_(8,
// 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
//
// Vector sigmas = repeat(8,1.0);
// JacobianFactor expected(combinedMeasurement, b, sigmas);
// EXPECT(assert_equal(expected,combinedFactor));
//}
#endif
/* ************************************************************************* */
TEST(GaussianFactor, eliminate2 )
{
@ -451,155 +168,6 @@ TEST(GaussianFactor, eliminate2 )
EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3));
}
/* ************************************************************************* */
TEST(GaussianFactor, eliminateFrontals)
{
// Augmented Ab test case for whole factor graph
Matrix Ab = Matrix_(14,11,
4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1.,
9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4.,
5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0.,
5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5.,
0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4.,
0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6.,
0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6.,
0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6.,
0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0.,
0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0.,
0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3.,
0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5.,
0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6.,
0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.);
// Create first factor (from pieces of Ab)
list<pair<Index, Matrix> > terms1;
terms1 +=
make_pair(3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))),
make_pair(5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))),
make_pair(7, ublas::project(Ab, ublas::range(0,4), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(0,4), ublas::range(6,8))),
make_pair(11, ublas::project(Ab, ublas::range(0,4), ublas::range(8,10)));
Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4));
JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5)));
// Create second factor
list<pair<Index, Matrix> > terms2;
terms2 +=
make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))),
make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(4,8), ublas::range(6,8))),
make_pair(11, ublas::project(Ab, ublas::range(4,8), ublas::range(8,10)));
Vector b2 = ublas::project(ublas::column(Ab, 10), ublas::range(4,8));
JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5)));
// Create third factor
list<pair<Index, Matrix> > terms3;
terms3 +=
make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))),
make_pair(11, ublas::project(Ab, ublas::range(8,12), ublas::range(8,10)));
Vector b3 = ublas::project(ublas::column(Ab, 10), ublas::range(8,12));
JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5)));
// Create fourth factor
list<pair<Index, Matrix> > terms4;
terms4 +=
make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10)));
Vector b4 = ublas::project(ublas::column(Ab, 10), ublas::range(12,14));
JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5)));
// Create factor graph
GaussianFactorGraph factors;
factors.push_back(factor1);
factors.push_back(factor2);
factors.push_back(factor3);
factors.push_back(factor4);
// Create combined factor
JacobianFactor combined(*JacobianFactor::Combine(*factors.dynamicCastFactors<FactorGraph<JacobianFactor> >(), VariableSlots(factors)));
// Copies factors as they will be eliminated in place
JacobianFactor actualFactor_QR = combined;
JacobianFactor actualFactor_Chol = combined;
// Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
Matrix R = 2.0*Matrix_(11,11,
-12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611,
0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625,
0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250,
0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676,
0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277,
0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769,
0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081,
0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685,
0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476,
0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635);
// Expected conditional on first variable from first 2 rows of R
Matrix R1 = ublas::project(R, ublas::range(0,2), ublas::range(0,2));
list<pair<Index, Matrix> > cterms1;
cterms1 +=
make_pair(5, ublas::project(R, ublas::range(0,2), ublas::range(2,4))),
make_pair(7, ublas::project(R, ublas::range(0,2), ublas::range(4,6))),
make_pair(9, ublas::project(R, ublas::range(0,2), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(0,2), ublas::range(8,10)));
Vector d1 = ublas::project(ublas::column(R, 10), ublas::range(0,2));
GaussianConditional::shared_ptr cond1(new GaussianConditional(3, d1, R1, cterms1, ones(2)));
// Expected conditional on second variable from next 2 rows of R
Matrix R2 = ublas::project(R, ublas::range(2,4), ublas::range(2,4));
list<pair<Index, Matrix> > cterms2;
cterms2 +=
make_pair(7, ublas::project(R, ublas::range(2,4), ublas::range(4,6))),
make_pair(9, ublas::project(R, ublas::range(2,4), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(2,4), ublas::range(8,10)));
Vector d2 = ublas::project(ublas::column(R, 10), ublas::range(2,4));
GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2)));
// Expected conditional on third variable from next 2 rows of R
Matrix R3 = ublas::project(R, ublas::range(4,6), ublas::range(4,6));
list<pair<Index, Matrix> > cterms3;
cterms3 +=
make_pair(9, ublas::project(R, ublas::range(4,6), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(4,6), ublas::range(8,10)));
Vector d3 = ublas::project(ublas::column(R, 10), ublas::range(4,6));
GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2)));
// Create expected Bayes net fragment from three conditionals above
GaussianBayesNet expectedFragment;
expectedFragment.push_back(cond1);
expectedFragment.push_back(cond2);
expectedFragment.push_back(cond3);
// Get expected matrices for remaining factor
Matrix Ae1 = ublas::project(R, ublas::range(6,10), ublas::range(6,8));
Matrix Ae2 = ublas::project(R, ublas::range(6,10), ublas::range(8,10));
Vector be = ublas::project(ublas::column(R, 10), ublas::range(6,10));
// Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3);
EXPECT(assert_equal(expectedFragment, actualFragment_QR, 0.001));
EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size()));
EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0]));
EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1]));
EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001));
EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001));
EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001));
EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001));
// Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!!
// GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactor::SOLVE_CHOLESKY);
// EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001));
// EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size()));
// EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0]));
// EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1]));
// EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); ////
// EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001));
// EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); ////
// EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001));
}
/* ************************************************************************* */
TEST(GaussianFactor, default_error )
{
@ -610,28 +178,29 @@ TEST(GaussianFactor, default_error )
EXPECT(actual==0.0);
}
////* ************************************************************************* */
//TEST(GaussianFactor, eliminate_empty )
//{
// // create an empty factor
// JacobianFactor f;
//
// // eliminate the empty factor
// GaussianConditional::shared_ptr actualCG;
// JacobianFactor::shared_ptr actualLF(new JacobianFactor(f));
// actualCG = actualLF->eliminateFirst();
//
// // expected Conditional Gaussian is just a parent-less node with P(x)=1
// GaussianConditional expectedCG(_x2_);
//
// // expected remaining factor is still empty :-)
// JacobianFactor expectedLF;
//
// // check if the result matches
// EXPECT(actualCG->equals(expectedCG));
// EXPECT(actualLF->equals(expectedLF));
//}
//* ************************************************************************* */
#ifdef BROKEN
TEST(GaussianFactor, eliminate_empty )
{
// create an empty factor
JacobianFactor f;
// eliminate the empty factor
GaussianConditional::shared_ptr actualCG;
JacobianFactor::shared_ptr actualLF(new JacobianFactor(f));
actualCG = actualLF->eliminateFirst();
// expected Conditional Gaussian is just a parent-less node with P(x)=1
GaussianConditional expectedCG(_x2_);
// expected remaining factor is still empty :-)
JacobianFactor expectedLF;
// check if the result matches
EXPECT(actualCG->equals(expectedCG));
EXPECT(actualLF->equals(expectedLF));
}
#endif
//* ************************************************************************* */
TEST(GaussianFactor, empty )
{
@ -648,29 +217,6 @@ void print(const list<T>& i) {
cout << endl;
}
///* ************************************************************************* */
//TEST(GaussianFactor, tally_separator )
//{
// JacobianFactor f(_x1_, eye(2), _x2_, eye(2), _l1_, eye(2), ones(2), sigma0_1);
//
// std::set<Index> act1, act2, act3;
// f.tally_separator(_x1_, act1);
// f.tally_separator(_x2_, act2);
// f.tally_separator(_l1_, act3);
//
// EXPECT(act1.size() == 2);
// EXPECT(act1.count(_x2_) == 1);
// EXPECT(act1.count(_l1_) == 1);
//
// EXPECT(act2.size() == 2);
// EXPECT(act2.count(_x1_) == 1);
// EXPECT(act2.count(_l1_) == 1);
//
// EXPECT(act3.size() == 2);
// EXPECT(act3.count(_x1_) == 1);
// EXPECT(act3.count(_x2_) == 1);
//}
/* ************************************************************************* */
TEST(GaussianFactor, CONSTRUCTOR_GaussianConditional )
{
@ -690,22 +236,6 @@ TEST(GaussianFactor, CONSTRUCTOR_GaussianConditional )
EXPECT(assert_equal(expectedLF,actualLF,1e-5));
}
///* ************************************************************************* */
//TEST(GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained )
//{
// Matrix Ax = eye(2);
// Vector b = Vector_(2, 3.0, 5.0);
// SharedDiagonal noisemodel = noiseModel::Constrained::All(2);
// JacobianFactor::shared_ptr expected(new JacobianFactor(_x0_, Ax, b, noisemodel));
// GaussianFactorGraph graph;
// graph.push_back(expected);
//
// GaussianConditional::shared_ptr conditional = GaussianSequentialSolver::EliminateUntil(graph,_x0_+1);
// JacobianFactor actual(*conditional);
//
// EXPECT(assert_equal(*expected, actual));
//}
/* ************************************************************************* */
TEST ( JacobianFactor, constraint_eliminate1 )
{
@ -768,103 +298,6 @@ TEST ( JacobianFactor, constraint_eliminate2 )
EXPECT(assert_equal(expectedCG, *actualCG, 1e-4));
}
/* ************************************************************************* */
TEST(GaussianFactor, permuteWithInverse)
{
Matrix A1 = Matrix_(2,2,
1.0, 2.0,
3.0, 4.0);
Matrix A2 = Matrix_(2,1,
5.0,
6.0);
Matrix A3 = Matrix_(2,3,
7.0, 8.0, 9.0,
10.0, 11.0, 12.0);
Vector b = Vector_(2, 13.0, 14.0);
Permutation inversePermutation(6);
inversePermutation[0] = 5;
inversePermutation[1] = 4;
inversePermutation[2] = 3;
inversePermutation[3] = 2;
inversePermutation[4] = 1;
inversePermutation[5] = 0;
JacobianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0));
GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual)));
VariableIndex actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse());
JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected)));
// GaussianVariableIndex expectedIndex(expectedFG);
EXPECT(assert_equal(expected, actual));
// // todo: fix this!!! VariableIndex should not hold slots
// for(Index j=0; j<actualIndex.size(); ++j) {
// BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, actualIndex[j]) {
// factor_pos.variablePosition = numeric_limits<Index>::max(); }
// }
// for(Index j=0; j<expectedIndex.size(); ++j) {
// BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, expectedIndex[j]) {
// factor_pos.variablePosition = numeric_limits<Index>::max(); }
// }
// EXPECT(assert_equal(expectedIndex, actualIndex));
}
///* ************************************************************************* */
//TEST(GaussianFactor, erase)
//{
// Vector b = Vector_(3, 1., 2., 3.);
// SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.));
// std::list<std::pair<Index, Matrix> > terms;
// terms.push_back(make_pair(_x0_, eye(2)));
// terms.push_back(make_pair(_x1_, 2.*eye(2)));
//
// JacobianFactor actual(terms, b, noise);
// int erased = actual.erase_A(_x0_);
//
// LONGS_EQUAL(1, erased);
// JacobianFactor expected(_x1_, 2.*eye(2), b, noise);
// EXPECT(assert_equal(expected, actual));
//}
///* ************************************************************************* */
//TEST(GaussianFactor, eliminateMatrix)
//{
// Matrix Ab = Matrix_(3, 4,
// 1., 2., 0., 3.,
// 0., 4., 5., 6.,
// 0., 0., 7., 8.);
// SharedDiagonal model(Vector_(3, 0.5, 0.5, 0.5));
// Ordering frontals; frontals += _x1_, _x2_;
// Ordering separator; separator += _x3_;
// Dimensions dimensions;
// dimensions.insert(make_pair(_x1_, 1));
// dimensions.insert(make_pair(_x2_, 1));
// dimensions.insert(make_pair(_x3_, 1));
//
// JacobianFactor::shared_ptr factor;
// GaussianBayesNet bn;
// boost::tie(bn, factor) =
// JacobianFactor::eliminateMatrix(Ab, NULL, model, frontals, separator, dimensions);
//
// GaussianBayesNet bn_expected;
// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(_x1_, Vector_(1, 6.), Matrix_(1, 1, 2.),
// _x2_, Matrix_(1, 1, 4.), _x3_, Matrix_(1, 1, 0.), Vector_(1, 1.)));
// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(_x2_, Vector_(1, 12.), Matrix_(1, 1, 8.),
// _x3_, Matrix_(1, 1, 10.), Vector_(1, 1.)));
// bn_expected.push_back(conditional1);
// bn_expected.push_back(conditional2);
// EXPECT(assert_equal(bn_expected, bn));
//
// JacobianFactor factor_expected(_x3_, Matrix_(1, 1, 14.), Vector_(1, 16.), SharedDiagonal(Vector_(1, 1.)));
// EXPECT(assert_equal(factor_expected, *factor));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

536
gtsam/linear/tests/testGaussianFactorGraph.cpp Normal file
View File

@ -0,0 +1,536 @@
/* ----------------------------------------------------------------------------
* 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 testGaussianFactor.cpp
* @brief Unit tests for Linear Factor
* @author Christian Potthast
* @author Frank Dellaert
**/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/linear/GaussianFactorGraph.h>
using namespace std;
using namespace gtsam;
using namespace boost;
namespace ublas = boost::numeric::ublas;
static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */
#ifdef BROKEN
TEST(GaussianFactor, Combine)
{
Matrix A00 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 0.0, 0.0, 0.0);
Vector s0 = Vector_(3, 0.0, 0.0, 0.0);
Matrix A10 = Matrix_(3,3,
0.0, -2.0, -4.0,
2.0, 0.0, 2.0,
0.0, 0.0, -10.0);
Matrix A11 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 10.0);
Vector b1 = Vector_(3, 6.0, 2.0, 0.0);
Vector s1 = Vector_(3, 1.0, 1.0, 1.0);
Matrix A20 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b2 = Vector_(3, 0.0, 0.0, 0.0);
Vector s2 = Vector_(3, 100.0, 100.0, 100.0);
GaussianFactorGraph gfg;
gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true));
GaussianVariableIndex varindex(gfg);
vector<size_t> factors(3); factors[0]=0; factors[1]=1; factors[2]=2;
vector<size_t> variables(2); variables[0]=0; variables[1]=1;
vector<vector<size_t> > variablePositions(3);
variablePositions[0].resize(1); variablePositions[0][0]=0;
variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1;
variablePositions[2].resize(1); variablePositions[2][0]=0;
JacobianFactor actual = *JacobianFactor::Combine(gfg, varindex, factors, variables, variablePositions);
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A00, &A10, &A20);
Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3);
Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
EXPECT(assert_equal(expected, actual));
}
#endif
/* ************************************************************************* */
TEST(GaussianFactorGraph, Combine2)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
JacobianFactor actual = *CombineJacobians(*gfg.dynamicCastFactors<FactorGraph<
JacobianFactor> > (), VariableSlots(gfg));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST_UNSAFE(GaussianFactor, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianBayesNet expectedBN(*expectedFactor.eliminate(1));
GaussianBayesNet::shared_ptr actualBN;
GaussianFactor::shared_ptr actualFactor;
boost::tie(actualBN, actualFactor) = //
EliminateQR(*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> > (), 1);
JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<
JacobianFactor>(actualFactor);
EXPECT(assert_equal(expectedBN, *actualBN));
EXPECT(assert_equal(expectedFactor, *actualJacobian));
}
/* ************************************************************************* */
#ifdef BROKEN
TEST( NonlinearFactorGraph, combine2){
double sigma1 = 0.0957;
Matrix A11(2,2);
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 1;
Vector b(2);
b(0) = 2; b(1) = -1;
JacobianFactor::shared_ptr f1(new JacobianFactor(_x1_, A11, b*sigma1, sharedSigma(2,sigma1)));
double sigma2 = 0.5;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 4 ; b(1) = -5;
JacobianFactor::shared_ptr f2(new JacobianFactor(_x1_, A11, b*sigma2, sharedSigma(2,sigma2)));
double sigma3 = 0.25;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 3 ; b(1) = -88;
JacobianFactor::shared_ptr f3(new JacobianFactor(_x1_, A11, b*sigma3, sharedSigma(2,sigma3)));
// TODO: find a real sigma value for this example
double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7;
b(0) = 5 ; b(1) = -6;
JacobianFactor::shared_ptr f4(new JacobianFactor(_x1_, A11*sigma4, b*sigma4, sharedSigma(2,sigma4)));
vector<JacobianFactor::shared_ptr> lfg;
lfg.push_back(f1);
lfg.push_back(f2);
lfg.push_back(f3);
lfg.push_back(f4);
JacobianFactor combined(lfg);
Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4);
Matrix A22(8,2);
A22(0,0) = 1; A22(0,1) = 0;
A22(1,0) = 0; A22(1,1) = 1;
A22(2,0) = 1; A22(2,1) = 0;
A22(3,0) = 0; A22(3,1) = -1;
A22(4,0) = 1; A22(4,1) = 0;
A22(5,0) = 0; A22(5,1) = -1;
A22(6,0) = 0.6; A22(6,1) = 0;
A22(7,0) = 0; A22(7,1) = 0.7;
Vector exb(8);
exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2;
exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4;
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(_x1_, A22));
JacobianFactor expected(meas, exb, sigmas);
EXPECT(assert_equal(expected,combined));
}
/* ************************************************************************* */
TEST(GaussianFactor, linearFactorN){
Matrix I = eye(2);
vector<JacobianFactor::shared_ptr> f;
SharedDiagonal model = sharedSigma(2,1.0);
f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, I, Vector_(2,
10.0, 5.0), model)));
f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, -10 * I,
_x2_, 10 * I, Vector_(2, 1.0, -2.0), model)));
f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x2_, -10 * I,
_x3_, 10 * I, Vector_(2, 1.5, -1.5), model)));
f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x3_, -10 * I,
_x4_, 10 * I, Vector_(2, 2.0, -1.0), model)));
JacobianFactor combinedFactor(f);
vector<pair<Index, Matrix> > combinedMeasurement;
combinedMeasurement.push_back(make_pair(_x1_, Matrix_(8,2,
1.0, 0.0,
0.0, 1.0,
-10.0, 0.0,
0.0,-10.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0)));
combinedMeasurement.push_back(make_pair(_x2_, Matrix_(8,2,
0.0, 0.0,
0.0, 0.0,
10.0, 0.0,
0.0, 10.0,
-10.0, 0.0,
0.0,-10.0,
0.0, 0.0,
0.0, 0.0)));
combinedMeasurement.push_back(make_pair(_x3_, Matrix_(8,2,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
10.0, 0.0,
0.0, 10.0,
-10.0, 0.0,
0.0,-10.0)));
combinedMeasurement.push_back(make_pair(_x4_, Matrix_(8,2,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
10.0, 0.0,
0.0,10.0)));
Vector b = Vector_(8,
10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
Vector sigmas = repeat(8,1.0);
JacobianFactor expected(combinedMeasurement, b, sigmas);
EXPECT(assert_equal(expected,combinedFactor));
}
#endif
/* ************************************************************************* */
TEST(GaussianFactor, eliminateFrontals)
{
// Augmented Ab test case for whole factor graph
Matrix Ab = Matrix_(14,11,
4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1.,
9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4.,
5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0.,
5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5.,
0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4.,
0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6.,
0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6.,
0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6.,
0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0.,
0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0.,
0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3.,
0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5.,
0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6.,
0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.);
// Create first factor (from pieces of Ab)
list<pair<Index, Matrix> > terms1;
terms1 +=
make_pair(3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))),
make_pair(5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))),
make_pair(7, ublas::project(Ab, ublas::range(0,4), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(0,4), ublas::range(6,8))),
make_pair(11, ublas::project(Ab, ublas::range(0,4), ublas::range(8,10)));
Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4));
JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5)));
// Create second factor
list<pair<Index, Matrix> > terms2;
terms2 +=
make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))),
make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(4,8), ublas::range(6,8))),
make_pair(11, ublas::project(Ab, ublas::range(4,8), ublas::range(8,10)));
Vector b2 = ublas::project(ublas::column(Ab, 10), ublas::range(4,8));
JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5)));
// Create third factor
list<pair<Index, Matrix> > terms3;
terms3 +=
make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))),
make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))),
make_pair(11, ublas::project(Ab, ublas::range(8,12), ublas::range(8,10)));
Vector b3 = ublas::project(ublas::column(Ab, 10), ublas::range(8,12));
JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5)));
// Create fourth factor
list<pair<Index, Matrix> > terms4;
terms4 +=
make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10)));
Vector b4 = ublas::project(ublas::column(Ab, 10), ublas::range(12,14));
JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5)));
// Create factor graph
GaussianFactorGraph factors;
factors.push_back(factor1);
factors.push_back(factor2);
factors.push_back(factor3);
factors.push_back(factor4);
// Create combined factor
JacobianFactor combined(*CombineJacobians(*factors.dynamicCastFactors<FactorGraph<
JacobianFactor> > (), VariableSlots(factors)));
// Copies factors as they will be eliminated in place
JacobianFactor actualFactor_QR = combined;
JacobianFactor actualFactor_Chol = combined;
// Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
Matrix R = 2.0*Matrix_(11,11,
-12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611,
0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625,
0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250,
0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676,
0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277,
0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769,
0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081,
0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685,
0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476,
0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635);
// Expected conditional on first variable from first 2 rows of R
Matrix R1 = ublas::project(R, ublas::range(0,2), ublas::range(0,2));
list<pair<Index, Matrix> > cterms1;
cterms1 +=
make_pair(5, ublas::project(R, ublas::range(0,2), ublas::range(2,4))),
make_pair(7, ublas::project(R, ublas::range(0,2), ublas::range(4,6))),
make_pair(9, ublas::project(R, ublas::range(0,2), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(0,2), ublas::range(8,10)));
Vector d1 = ublas::project(ublas::column(R, 10), ublas::range(0,2));
GaussianConditional::shared_ptr cond1(new GaussianConditional(3, d1, R1, cterms1, ones(2)));
// Expected conditional on second variable from next 2 rows of R
Matrix R2 = ublas::project(R, ublas::range(2,4), ublas::range(2,4));
list<pair<Index, Matrix> > cterms2;
cterms2 +=
make_pair(7, ublas::project(R, ublas::range(2,4), ublas::range(4,6))),
make_pair(9, ublas::project(R, ublas::range(2,4), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(2,4), ublas::range(8,10)));
Vector d2 = ublas::project(ublas::column(R, 10), ublas::range(2,4));
GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2)));
// Expected conditional on third variable from next 2 rows of R
Matrix R3 = ublas::project(R, ublas::range(4,6), ublas::range(4,6));
list<pair<Index, Matrix> > cterms3;
cterms3 +=
make_pair(9, ublas::project(R, ublas::range(4,6), ublas::range(6,8))),
make_pair(11, ublas::project(R, ublas::range(4,6), ublas::range(8,10)));
Vector d3 = ublas::project(ublas::column(R, 10), ublas::range(4,6));
GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2)));
// Create expected Bayes net fragment from three conditionals above
GaussianBayesNet expectedFragment;
expectedFragment.push_back(cond1);
expectedFragment.push_back(cond2);
expectedFragment.push_back(cond3);
// Get expected matrices for remaining factor
Matrix Ae1 = ublas::project(R, ublas::range(6,10), ublas::range(6,8));
Matrix Ae2 = ublas::project(R, ublas::range(6,10), ublas::range(8,10));
Vector be = ublas::project(ublas::column(R, 10), ublas::range(6,10));
// Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3);
EXPECT(assert_equal(expectedFragment, actualFragment_QR, 0.001));
EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size()));
EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0]));
EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1]));
EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001));
EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001));
EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001));
EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001));
// Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!!
#ifdef BROKEN
GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactor::SOLVE_CHOLESKY);
EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001));
EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size()));
EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0]));
EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1]));
EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); ////
EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001));
EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); ////
EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001));
#endif
}
/* ************************************************************************* */
#ifdef BROKEN
TEST(GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained )
{
Matrix Ax = eye(2);
Vector b = Vector_(2, 3.0, 5.0);
SharedDiagonal noisemodel = noiseModel::Constrained::All(2);
JacobianFactor::shared_ptr expected(new JacobianFactor(_x0_, Ax, b, noisemodel));
GaussianFactorGraph graph;
graph.push_back(expected);
GaussianConditional::shared_ptr conditional = GaussianSequentialSolver::EliminateUntil(graph,_x0_+1);
JacobianFactor actual(*conditional);
EXPECT(assert_equal(*expected, actual));
}
#endif
/* ************************************************************************* */
TEST(GaussianFactor, permuteWithInverse)
{
Matrix A1 = Matrix_(2,2,
1.0, 2.0,
3.0, 4.0);
Matrix A2 = Matrix_(2,1,
5.0,
6.0);
Matrix A3 = Matrix_(2,3,
7.0, 8.0, 9.0,
10.0, 11.0, 12.0);
Vector b = Vector_(2, 13.0, 14.0);
Permutation inversePermutation(6);
inversePermutation[0] = 5;
inversePermutation[1] = 4;
inversePermutation[2] = 3;
inversePermutation[3] = 2;
inversePermutation[4] = 1;
inversePermutation[5] = 0;
JacobianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0));
GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual)));
VariableIndex actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse());
JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected)));
// GaussianVariableIndex expectedIndex(expectedFG);
EXPECT(assert_equal(expected, actual));
#ifdef BROKEN
// todo: fix this!!! VariableIndex should not hold slots
for(Index j=0; j<actualIndex.size(); ++j) {
BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, actualIndex[j]) {
factor_pos.variablePosition = numeric_limits<Index>::max(); }
}
for(Index j=0; j<expectedIndex.size(); ++j) {
BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, expectedIndex[j]) {
factor_pos.variablePosition = numeric_limits<Index>::max(); }
}
EXPECT(assert_equal(expectedIndex, actualIndex));
#endif
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

4
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -69,7 +69,7 @@ TEST( GaussianJunctionTree, eliminate )
{
GaussianFactorGraph fg = createChain();
GaussianJunctionTree junctionTree(fg);
BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate();
BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate(&EliminateQR);
typedef BayesTree<GaussianConditional>::sharedConditional sharedConditional;
Matrix two = Matrix_(1,1,2.);
@ -90,7 +90,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
GaussianFactorGraph fg = createChain();
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize();
VectorValues actual = tree.optimize(&EliminateQR);
VectorValues expected(vector<size_t>(4,1));
expected[x1] = Vector_(1, 0.);
expected[x2] = Vector_(1, 1.);

23
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -129,11 +129,13 @@ TEST(GaussianFactor, CombineAndEliminate)
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianBayesNet expectedBN(*expectedFactor.eliminate(1));
pair<GaussianBayesNet::shared_ptr, HessianFactor::shared_ptr> actualCholesky(HessianFactor::CombineAndEliminate(
*gfg.convertCastFactors<FactorGraph<HessianFactor> >(), 1));
GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(
*gfg.convertCastFactors<FactorGraph<HessianFactor> > (), 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actualCholesky.second);
EXPECT(assert_equal(expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(HessianFactor(expectedFactor), *actualCholesky.second, 1e-6));
EXPECT(assert_equal(HessianFactor(expectedFactor), *actualFactor, 1e-6));
}
/* ************************************************************************* */
@ -177,8 +179,11 @@ TEST(GaussianFactor, eliminate2 )
HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
FactorGraph<HessianFactor> combinedLFG_Chol;
combinedLFG_Chol.push_back(combinedLF_Chol);
pair<GaussianBayesNet::shared_ptr, HessianFactor::shared_ptr> actual_Chol =
HessianFactor::CombineAndEliminate(combinedLFG_Chol, 1);
GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky(
combinedLFG_Chol, 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actual_Chol.second);
// create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit
@ -203,7 +208,7 @@ TEST(GaussianFactor, eliminate2 )
)/sigma;
Vector b1 = Vector_(2,0.0,0.894427);
JacobianFactor expectedLF(1, Bl1x1, b1, repeat(2,1.0));
EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3));
}
/* ************************************************************************* */
@ -276,11 +281,13 @@ TEST(GaussianFactor, eliminateUnsorted) {
GaussianBayesNet::shared_ptr expected_bn;
GaussianFactor::shared_ptr expected_factor;
boost::tie(expected_bn, expected_factor) = GaussianFactor::CombineAndEliminate(sortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY);
boost::tie(expected_bn, expected_factor) =
EliminatePreferCholesky(sortedGraph, 1);
GaussianBayesNet::shared_ptr actual_bn;
GaussianFactor::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) = GaussianFactor::CombineAndEliminate(unsortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY);
boost::tie(actual_bn, actual_factor) =
EliminatePreferCholesky(unsortedGraph, 1);
CHECK(assert_equal(*expected_bn, *actual_bn, 1e-10));
CHECK(assert_equal(*expected_factor, *actual_factor, 1e-10));

4
gtsam/linear/tests/timeFactorOverhead.cpp
View File

@ -82,7 +82,7 @@ int main(int argc, char *argv[]) {
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate());
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate(&EliminateQR));
VectorValues soln(optimize(*gbn));
}
blocksolve = timer.elapsed();
@ -126,7 +126,7 @@ int main(int argc, char *argv[]) {
cout.flush();
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate());
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate(&EliminateQR));
VectorValues soln(optimize(*gbn));
}
combsolve = timer.elapsed();

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

@ -28,7 +28,8 @@ using namespace gtsam;
/* ************************************************************************* */
template<class Values>
void NonlinearISAM<Values>::update(const Factors& newFactors, const Values& initialValues) {
void NonlinearISAM<Values>::update(const Factors& newFactors,
const Values& initialValues) {
if(newFactors.size() > 0) {

14
tests/testGaussianISAM.cpp
View File

@ -125,12 +125,12 @@ TEST( BayesTree, linear_smoother_shortcuts )
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
GaussianISAM::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut(R);
GaussianBayesNet actual1 = GaussianISAM::shortcut(R,R);
CHECK(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = bayesTree[ordering["x5"]];
GaussianBayesNet actual2 = C2->shortcut(R);
GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
CHECK(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root)
@ -139,7 +139,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected3;
push_front(expected3,ordering["x5"], zero(2), eye(2)/sigma3, ordering["x6"], A56/sigma3, ones(2));
GaussianISAM::sharedClique C3 = bayesTree[ordering["x4"]];
GaussianBayesNet actual3 = C3->shortcut(R);
GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
CHECK(assert_equal(expected3,actual3,tol));
// Check the conditional P(C4|Root)
@ -148,7 +148,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected4;
push_front(expected4, ordering["x4"], zero(2), eye(2)/sigma4, ordering["x6"], A46/sigma4, ones(2));
GaussianISAM::sharedClique C4 = bayesTree[ordering["x3"]];
GaussianBayesNet actual4 = C4->shortcut(R);
GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R);
CHECK(assert_equal(expected4,actual4,tol));
}
@ -264,19 +264,19 @@ TEST( BayesTree, balanced_smoother_shortcuts )
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
GaussianISAM::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut(R);
GaussianBayesNet actual1 = GaussianISAM::shortcut(R,R);
CHECK(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = bayesTree[ordering["x3"]];
GaussianBayesNet actual2 = C2->shortcut(R);
GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
CHECK(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet[ordering["x2"]];
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
GaussianISAM::sharedClique C3 = bayesTree[ordering["x1"]];
GaussianBayesNet actual3 = C3->shortcut(R);
GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
CHECK(assert_equal(expected3,actual3,tol));
}

6
tests/testGaussianJunctionTree.cpp
View File

@ -91,7 +91,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
// optimize the graph
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize();
VectorValues actual = tree.optimize(&EliminateQR);
// verify
VectorValues expected(vector<size_t>(7,2)); // expected solution
@ -112,7 +112,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
// optimize the graph
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize();
VectorValues actual = tree.optimize(&EliminateQR);
// verify
VectorValues expected = createCorrectDelta(ordering); // expected solution
@ -174,7 +174,7 @@ TEST(GaussianJunctionTree, slamlike) {
GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering);
GaussianJunctionTree gjt(linearized);
VectorValues deltaactual = gjt.optimize();
VectorValues deltaactual = gjt.optimize(&EliminateQR);
planarSLAM::Values actual = init.expmap(deltaactual, ordering);
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();

3
tests/testSymbolicBayesNet.cpp
View File

@ -56,7 +56,8 @@ TEST( SymbolicBayesNet, constructor )
SymbolicFactorGraph fg(factorGraph);
// eliminate it
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(
&EliminateSymbolic);
CHECK(assert_equal(expected, actual));
}

2
tests/testSymbolicFactorGraph.cpp
View File

@ -143,7 +143,7 @@ TEST( SymbolicFactorGraph, eliminate )
SymbolicFactorGraph fg(factorGraph);
// eliminate it
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(&EliminateSymbolic);
CHECK(assert_equal(expected,actual));
}