Moved inference methods to new compilation unit. Added [factor], and [marginalize] now returns a factor graph.
Frank Dellaert
parent
c85f5445db
commit
f677341108
43
.cproject
43
.cproject
|
|
@ -300,7 +300,6 @@
|
|||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -308,7 +307,6 @@
|
|||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -324,7 +322,6 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -340,6 +337,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -347,7 +345,6 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -355,6 +352,7 @@
|
|||
</target>
|
||||
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConditionalGaussian.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -362,23 +360,13 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-f local.mk</buildArguments>
|
||||
<buildTarget>testFGConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -386,6 +374,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -393,7 +382,6 @@
|
|||
</target>
|
||||
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -401,7 +389,6 @@
|
|||
</target>
|
||||
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -409,6 +396,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -416,7 +404,6 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -424,6 +411,7 @@
|
|||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -431,6 +419,7 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -438,7 +427,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -446,7 +434,6 @@
|
|||
</target>
|
||||
<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -454,7 +441,6 @@
|
|||
</target>
|
||||
<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -462,7 +448,6 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -470,6 +455,7 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -477,7 +463,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -485,6 +470,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -492,7 +478,6 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -500,14 +485,22 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testInference.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -515,6 +508,7 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -522,6 +516,7 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file BayesNet-inl.h
|
||||
* @brief Bayes chain template definitions
|
||||
* @brief Bayes net template definitions
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
|
|
@ -70,29 +70,5 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor, class Conditional>
|
||||
BayesNet<Conditional> marginals(const BayesNet<Conditional>& bn, const Ordering& keys) {
|
||||
// Convert to factor graph
|
||||
FactorGraph<Factor> factorGraph(bn);
|
||||
|
||||
// Get the keys of all variables and remove all keys we want the marginal for
|
||||
Ordering ord = bn.ordering();
|
||||
BOOST_FOREACH(string key, keys) ord.remove(key); // TODO: O(n*k), faster possible?
|
||||
|
||||
// add marginal keys at end
|
||||
BOOST_FOREACH(string key, keys) ord.push_back(key);
|
||||
|
||||
// eliminate to get joint
|
||||
BayesNet<Conditional> joint = _eliminate<Factor,Conditional>(factorGraph,ord);
|
||||
|
||||
// remove all integrands, P(K) = \int_I P(I|K) P(K)
|
||||
size_t nrIntegrands = ord.size()-keys.size();
|
||||
for(int i=0;i<nrIntegrands;i++) joint.pop_front();
|
||||
|
||||
// joint is now only on keys, return it
|
||||
return joint;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -105,13 +105,4 @@ namespace gtsam {
|
|||
}
|
||||
}; // BayesNet
|
||||
|
||||
/** doubly templated functions */
|
||||
|
||||
/**
|
||||
* integrate out all except ordering, might be inefficient as the ordering
|
||||
* will simply be the current ordering with the keys put in the back
|
||||
*/
|
||||
template<class Factor, class Conditional>
|
||||
BayesNet<Conditional> marginals(const BayesNet<Conditional>& bn, const Ordering& keys);
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -9,8 +9,7 @@
|
|||
using namespace boost::assign;
|
||||
|
||||
#include "BayesTree.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "BayesNet-inl.h"
|
||||
#include "inference-inl.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -106,7 +105,7 @@ namespace gtsam {
|
|||
BOOST_REVERSE_FOREACH(string key, integrands) ordering.push_front(key);
|
||||
|
||||
// eliminate to get marginal
|
||||
BayesNet<Conditional> p_S_R = _eliminate<Factor,Conditional>(p_Cp_R,ordering);
|
||||
BayesNet<Conditional> p_S_R = eliminate<Factor,Conditional>(p_Cp_R,ordering);
|
||||
|
||||
// remove all integrands
|
||||
BOOST_FOREACH(string key, integrands) p_S_R.pop_front();
|
||||
|
|
@ -123,7 +122,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
BayesNet<Conditional>
|
||||
FactorGraph<Factor>
|
||||
BayesTree<Conditional>::Clique::marginal(shared_ptr R) {
|
||||
// If we are the root, just return this root
|
||||
if (R.get()==this) return *R;
|
||||
|
|
@ -134,7 +133,7 @@ namespace gtsam {
|
|||
p_FSR.push_back(*R);
|
||||
|
||||
// Find marginal on the keys we are interested in
|
||||
return marginals<Factor>(p_FSR,keys());
|
||||
return marginalize<Factor,Conditional>(p_FSR,keys());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -142,7 +141,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
BayesNet<Conditional>
|
||||
FactorGraph<Factor>
|
||||
BayesTree<Conditional>::Clique::joint(shared_ptr C2, shared_ptr R) {
|
||||
// For now, assume neither is the root
|
||||
|
||||
|
|
@ -160,7 +159,7 @@ namespace gtsam {
|
|||
keys12.unique();
|
||||
|
||||
// Calculate the marginal
|
||||
return marginals<Factor>(*bn,keys12);
|
||||
return marginalize<Factor,Conditional>(*bn,keys12);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -226,17 +225,35 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
BayesNet<Conditional>
|
||||
FactorGraph<Factor>
|
||||
BayesTree<Conditional>::marginal(const string& key) const {
|
||||
|
||||
// get clique containing key
|
||||
sharedClique clique = (*this)[key];
|
||||
|
||||
// calculate or retrieve its marginal
|
||||
BayesNet<Conditional> cliqueMarginal = clique->marginal<Factor>(root_);
|
||||
FactorGraph<Factor> cliqueMarginal = clique->marginal<Factor>(root_);
|
||||
|
||||
// Get the marginal on the single key
|
||||
return marginals<Factor>(cliqueMarginal,Ordering(key));
|
||||
// create an ordering where only the requested key is not eliminated
|
||||
Ordering ord = clique->keys();
|
||||
ord.remove(key);
|
||||
|
||||
// partially eliminate, remaining factor graph is requested marginal
|
||||
eliminate<Factor,Conditional>(cliqueMarginal,ord);
|
||||
return cliqueMarginal;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
BayesNet<Conditional>
|
||||
BayesTree<Conditional>::marginalBayesNet(const string& key) const {
|
||||
|
||||
// calculate marginal as a factor graph
|
||||
FactorGraph<Factor> fg = this->marginal<Factor>(key);
|
||||
|
||||
// eliminate further to Bayes net
|
||||
return eliminate<Factor,Conditional>(fg,Ordering(key));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -244,19 +261,39 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
BayesNet<Conditional>
|
||||
FactorGraph<Factor>
|
||||
BayesTree<Conditional>::joint(const std::string& key1, const std::string& key2) const {
|
||||
|
||||
// get clique C1 and C2
|
||||
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
|
||||
|
||||
// calculate joint
|
||||
BayesNet<Conditional> p_C1C2 = C1->joint<Factor>(C2,root_);
|
||||
FactorGraph<Factor> p_C1C2 = C1->joint<Factor>(C2,root_);
|
||||
|
||||
// Get the marginal on the two keys
|
||||
// create an ordering where both requested keys are not eliminated
|
||||
Ordering ord = p_C1C2.keys();
|
||||
ord.remove(key1);
|
||||
ord.remove(key2);
|
||||
|
||||
// partially eliminate, remaining factor graph is requested joint
|
||||
// TODO, make eliminate functional
|
||||
eliminate<Factor,Conditional>(p_C1C2,ord);
|
||||
return p_C1C2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
BayesNet<Conditional>
|
||||
BayesTree<Conditional>::jointBayesNet(const std::string& key1, const std::string& key2) const {
|
||||
|
||||
// calculate marginal as a factor graph
|
||||
FactorGraph<Factor> fg = this->joint<Factor>(key1,key2);
|
||||
|
||||
// eliminate further to Bayes net
|
||||
Ordering ordering;
|
||||
ordering += key1, key2;
|
||||
return marginals<Factor>(p_C1C2,ordering);
|
||||
return eliminate<Factor,Conditional>(fg,ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
#include <boost/serialization/list.hpp>
|
||||
|
||||
#include "Testable.h"
|
||||
#include "FactorGraph.h"
|
||||
#include "BayesNet.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -70,11 +71,11 @@ namespace gtsam {
|
|||
|
||||
/** return the marginal P(C) of the clique */
|
||||
template<class Factor>
|
||||
BayesNet<Conditional> marginal(shared_ptr root);
|
||||
FactorGraph<Factor> marginal(shared_ptr root);
|
||||
|
||||
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
|
||||
template<class Factor>
|
||||
BayesNet<Conditional> joint(shared_ptr C2, shared_ptr root);
|
||||
FactorGraph<Factor> joint(shared_ptr C2, shared_ptr root);
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Clique> sharedClique;
|
||||
|
|
@ -142,11 +143,19 @@ namespace gtsam {
|
|||
|
||||
/** return marginal on any variable */
|
||||
template<class Factor>
|
||||
BayesNet<Conditional> marginal(const std::string& key) const;
|
||||
FactorGraph<Factor> marginal(const std::string& key) const;
|
||||
|
||||
/** return marginal on any variable, as a Bayes Net */
|
||||
template<class Factor>
|
||||
BayesNet<Conditional> marginalBayesNet(const std::string& key) const;
|
||||
|
||||
/** return joint on two variables */
|
||||
template<class Factor>
|
||||
BayesNet<Conditional> joint(const std::string& key1, const std::string& key2) const;
|
||||
FactorGraph<Factor> joint(const std::string& key1, const std::string& key2) const;
|
||||
|
||||
/** return joint on two variables as a BayesNet */
|
||||
template<class Factor>
|
||||
BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@
|
|||
#include "Ordering.h"
|
||||
#include "FactorGraph.h"
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -95,6 +98,16 @@ void FactorGraph<Factor>::push_back(sharedFactor factor) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
Ordering FactorGraph<Factor>::keys() const {
|
||||
string key;
|
||||
Ordering keys;
|
||||
list<int> indices_key;
|
||||
FOREACH_PAIR(key, indices_key, indices_) keys.push_back(key);
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Call colamd given a column-major symbolic matrix A
|
||||
|
|
@ -213,48 +226,6 @@ removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const string& key)
|
|||
return new_factor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* eliminate one node from the factor graph */
|
||||
/* ************************************************************************* */
|
||||
template<class Factor,class Conditional>
|
||||
boost::shared_ptr<Conditional> _eliminateOne(FactorGraph<Factor>& graph, const string& key) {
|
||||
|
||||
// combine the factors of all nodes connected to the variable to be eliminated
|
||||
// if no factors are connected to key, returns an empty factor
|
||||
boost::shared_ptr<Factor> joint_factor = removeAndCombineFactors(graph,key);
|
||||
|
||||
// eliminate that joint factor
|
||||
boost::shared_ptr<Factor> factor;
|
||||
boost::shared_ptr<Conditional> conditional;
|
||||
boost::tie(conditional, factor) = joint_factor->eliminate(key);
|
||||
|
||||
// add new factor on separator back into the graph
|
||||
if (!factor->empty()) graph.push_back(factor);
|
||||
|
||||
// return the conditional Gaussian
|
||||
return conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// This doubly templated function is generic. There is a LinearFactorGraph
|
||||
// version that returns a more specific GaussianBayesNet.
|
||||
// Note, you will need to include this file to instantiate the function.
|
||||
// TODO: get rid of summy argument
|
||||
/* ************************************************************************* */
|
||||
template<class Factor,class Conditional>
|
||||
BayesNet<Conditional>
|
||||
_eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering)
|
||||
{
|
||||
BayesNet<Conditional> bayesNet; // empty
|
||||
|
||||
BOOST_FOREACH(string key, ordering) {
|
||||
boost::shared_ptr<Conditional> cg = _eliminateOne<Factor,Conditional>(factorGraph,key);
|
||||
bayesNet.push_back(cg);
|
||||
}
|
||||
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) {
|
||||
|
|
|
|||
|
|
@ -86,6 +86,9 @@ namespace gtsam {
|
|||
/** Add a factor */
|
||||
void push_back(sharedFactor factor);
|
||||
|
||||
/** return keys in some random order */
|
||||
Ordering keys() const;
|
||||
|
||||
/**
|
||||
* Compute colamd ordering
|
||||
*/
|
||||
|
|
@ -124,27 +127,7 @@ namespace gtsam {
|
|||
template<class Factor> boost::shared_ptr<Factor>
|
||||
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const std::string& key);
|
||||
|
||||
/** doubly templated functions */
|
||||
|
||||
/**
|
||||
* Eliminate a single node yielding a Conditional
|
||||
* Eliminates the factors from the factor graph through findAndRemoveFactors
|
||||
* and adds a new factor on the separator to the factor graph
|
||||
*/
|
||||
template<class Factor, class Conditional>
|
||||
boost::shared_ptr<Conditional> _eliminateOne(
|
||||
FactorGraph<Factor>& factorGraph,
|
||||
const std::string& key);
|
||||
|
||||
/**
|
||||
* eliminate factor graph using the given (not necessarily complete)
|
||||
* ordering, yielding a chordal Bayes net and (partially eliminated) FG
|
||||
*/
|
||||
template<class Factor, class Conditional>
|
||||
BayesNet<Conditional>
|
||||
_eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering);
|
||||
|
||||
/**
|
||||
* static function that combines two factor graphs
|
||||
* @param const &fg1 Linear factor graph
|
||||
* @param const &fg2 Linear factor graph
|
||||
|
|
|
|||
|
|
@ -12,9 +12,10 @@
|
|||
|
||||
#include <colamd/colamd.h>
|
||||
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "LinearFactorGraph.h"
|
||||
#include "LinearFactorSet.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "inference-inl.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -40,6 +41,12 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
|
|||
return separator;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConditionalGaussian::shared_ptr
|
||||
LinearFactorGraph::eliminateOne(const std::string& key) {
|
||||
return gtsam::eliminateOne<LinearFactor,ConditionalGaussian>(*this, key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianBayesNet
|
||||
LinearFactorGraph::eliminate(const Ordering& ordering)
|
||||
|
|
@ -59,15 +66,19 @@ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
|
|||
GaussianBayesNet chordalBayesNet = eliminate(ordering);
|
||||
|
||||
// calculate new configuration (using backsubstitution)
|
||||
VectorConfig newConfig = ::optimize(chordalBayesNet);
|
||||
|
||||
return newConfig;
|
||||
return ::optimize(chordalBayesNet);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianBayesNet>
|
||||
LinearFactorGraph::eliminate_(const Ordering& ordering) {
|
||||
return boost::shared_ptr<GaussianBayesNet>(new GaussianBayesNet(eliminate(ordering)));
|
||||
LinearFactorGraph::eliminate_(const Ordering& ordering)
|
||||
{
|
||||
boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
|
||||
BOOST_FOREACH(string key, ordering) {
|
||||
ConditionalGaussian::shared_ptr cg = eliminateOne(key);
|
||||
chordalBayesNet->push_back(cg);
|
||||
}
|
||||
return chordalBayesNet;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -66,9 +66,7 @@ namespace gtsam {
|
|||
* Eliminates the factors from the factor graph through findAndRemoveFactors
|
||||
* and adds a new factor on the separator to the factor graph
|
||||
*/
|
||||
inline ConditionalGaussian::shared_ptr eliminateOne(const std::string& key){
|
||||
return _eliminateOne<LinearFactor,ConditionalGaussian>(*this, key);
|
||||
}
|
||||
ConditionalGaussian::shared_ptr eliminateOne(const std::string& key);
|
||||
|
||||
/**
|
||||
* eliminate factor graph in place(!) in the given order, yielding
|
||||
|
|
|
|||
|
|
@ -7,9 +7,9 @@
|
|||
|
||||
#include <boost/foreach.hpp>
|
||||
#include "Ordering.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "SymbolicFactorGraph.h"
|
||||
#include "SymbolicBayesNet.h"
|
||||
#include "inference-inl.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -18,6 +18,12 @@ namespace gtsam {
|
|||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class FactorGraph<SymbolicFactor>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<SymbolicConditional>
|
||||
SymbolicFactorGraph::eliminateOne(const std::string& key){
|
||||
return gtsam::eliminateOne<SymbolicFactor,SymbolicConditional>(*this, key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicBayesNet
|
||||
SymbolicFactorGraph::eliminate(const Ordering& ordering)
|
||||
|
|
@ -26,7 +32,7 @@ namespace gtsam {
|
|||
|
||||
BOOST_FOREACH(string key, ordering) {
|
||||
SymbolicConditional::shared_ptr conditional =
|
||||
_eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key);
|
||||
gtsam::eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key);
|
||||
bayesNet.push_back(conditional);
|
||||
}
|
||||
return bayesNet;
|
||||
|
|
|
|||
|
|
@ -46,9 +46,7 @@ namespace gtsam {
|
|||
* Eliminates the factors from the factor graph through findAndRemoveFactors
|
||||
* and adds a new factor on the separator to the factor graph
|
||||
*/
|
||||
inline boost::shared_ptr<SymbolicConditional> eliminateOne(const std::string& key){
|
||||
return _eliminateOne<SymbolicFactor,SymbolicConditional>(*this, key);
|
||||
}
|
||||
boost::shared_ptr<SymbolicConditional> eliminateOne(const std::string& key);
|
||||
|
||||
/**
|
||||
* eliminate factor graph in place(!) in the given order, yielding
|
||||
|
|
|
|||
|
|
@ -177,28 +177,28 @@ TEST( BayesTree, balanced_smoother_marginals )
|
|||
LONGS_EQUAL(7,bayesTree.size());
|
||||
|
||||
// Check marginal on x1
|
||||
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
GaussianBayesNet actual1 = bayesTree.marginal<LinearFactor>("x1");
|
||||
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<LinearFactor>("x1");
|
||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||
|
||||
// Check marginal on x2
|
||||
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2);
|
||||
GaussianBayesNet actual2 = bayesTree.marginal<LinearFactor>("x2");
|
||||
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<LinearFactor>("x2");
|
||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||
|
||||
// Check marginal on x3
|
||||
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
|
||||
GaussianBayesNet actual3 = bayesTree.marginal<LinearFactor>("x3");
|
||||
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<LinearFactor>("x3");
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
// Check marginal on x4
|
||||
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
|
||||
GaussianBayesNet actual4 = bayesTree.marginal<LinearFactor>("x4");
|
||||
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<LinearFactor>("x4");
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
|
||||
// Check marginal on x7 (should be equal to x1)
|
||||
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
|
||||
GaussianBayesNet actual7 = bayesTree.marginal<LinearFactor>("x7");
|
||||
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<LinearFactor>("x7");
|
||||
CHECK(assert_equal(expected7,actual7,1e-4));
|
||||
}
|
||||
|
||||
|
|
@ -252,7 +252,8 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
|
|||
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x1", zero(2), eye(2), "x2", A12, sigma));
|
||||
expected.push_front(cg);
|
||||
Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
||||
GaussianBayesNet actual = C3->marginal<LinearFactor>(R);
|
||||
FactorGraph<LinearFactor> marginal = C3->marginal<LinearFactor>(R);
|
||||
GaussianBayesNet actual = eliminate<LinearFactor,ConditionalGaussian>(marginal,C3->keys());
|
||||
CHECK(assert_equal(expected,actual,1e-4));
|
||||
}
|
||||
|
||||
|
|
@ -276,14 +277,14 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
|
||||
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1", zero(2), eye(2), "x7", A, sigma));
|
||||
expected1.push_front(cg1);
|
||||
GaussianBayesNet actual1 = bayesTree.joint<LinearFactor>("x1","x7");
|
||||
GaussianBayesNet actual1 = bayesTree.jointBayesNet<LinearFactor>("x1","x7");
|
||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||
|
||||
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
|
||||
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("x7", zero(2), eye(2), "x1", A, sigma));
|
||||
expected2.push_front(cg2);
|
||||
GaussianBayesNet actual2 = bayesTree.joint<LinearFactor>("x7","x1");
|
||||
GaussianBayesNet actual2 = bayesTree.jointBayesNet<LinearFactor>("x7","x1");
|
||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||
|
||||
// Check the joint density P(x1,x4), i.e. with a root variable
|
||||
|
|
@ -292,7 +293,7 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
Matrix A14 = (-0.0769231)*eye(2);
|
||||
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x1", zero(2), eye(2), "x4", A14, sigma14));
|
||||
expected3.push_front(cg3);
|
||||
GaussianBayesNet actual3 = bayesTree.joint<LinearFactor>("x1","x4");
|
||||
GaussianBayesNet actual3 = bayesTree.jointBayesNet<LinearFactor>("x1","x4");
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
|
||||
|
|
@ -301,7 +302,7 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
Matrix A41 = (-0.055794)*eye(2);
|
||||
ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x1", A41, sigma41));
|
||||
expected4.push_front(cg4);
|
||||
GaussianBayesNet actual4 = bayesTree.joint<LinearFactor>("x4","x1");
|
||||
GaussianBayesNet actual4 = bayesTree.jointBayesNet<LinearFactor>("x4","x1");
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ using namespace boost::assign;
|
|||
#endif //HAVE_BOOST_SERIALIZATION
|
||||
|
||||
#include "GaussianBayesNet.h"
|
||||
#include "BayesNet-inl.h"
|
||||
#include "BayesNet.h"
|
||||
#include "smallExample.h"
|
||||
#include "Ordering.h"
|
||||
|
||||
|
|
@ -84,19 +84,6 @@ TEST( GaussianBayesNet, optimize )
|
|||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianBayesNet, marginals )
|
||||
{
|
||||
// create and marginalize a small Bayes net on "x"
|
||||
GaussianBayesNet cbn = createSmallGaussianBayesNet();
|
||||
Ordering keys("x");
|
||||
GaussianBayesNet actual = marginals<LinearFactor>(cbn,keys);
|
||||
|
||||
// expected is just scalar Gaussian on x
|
||||
GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2));
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef HAVE_BOOST_SERIALIZATION
|
||||
TEST( GaussianBayesNet, serialize )
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@ using namespace boost::assign;
|
|||
#include "Ordering.h"
|
||||
#include "smallExample.h"
|
||||
#include "GaussianBayesNet.h"
|
||||
#include <FactorGraph-inl.h> // needed for FactorGraph::eliminate
|
||||
#include "inference-inl.h" // needed for eliminate and marginals
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
|
|
@ -420,7 +420,7 @@ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet )
|
|||
|
||||
// Base FactorGraph only
|
||||
FactorGraph<LinearFactor> fg3(CBN);
|
||||
GaussianBayesNet CBN3 = _eliminate<LinearFactor,ConditionalGaussian>(fg3,ord);
|
||||
GaussianBayesNet CBN3 = gtsam::eliminate<LinearFactor,ConditionalGaussian>(fg3,ord);
|
||||
CHECK(assert_equal(CBN,CBN3));
|
||||
}
|
||||
|
||||
|
|
@ -585,6 +585,15 @@ TEST( LinearFactorGraph, variables )
|
|||
CHECK(expected==actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearFactorGraph, keys )
|
||||
{
|
||||
LinearFactorGraph fg = createLinearFactorGraph();
|
||||
Ordering expected;
|
||||
expected += "l1","x1","x2";
|
||||
CHECK(assert_equal(expected,fg.keys()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Tests ported from ConstrainedLinearFactorGraph
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue