BIG: eliminate and eliminateOne now doubly templated functions, not methods.
Minor: Standardized on new shared_ptr naming convention: shared_factor -> sharedFactor conditional_ptr -> sharedConditional node_ptr -> sharedCliquerelease/4.3a0
Frank Dellaert
parent
f5d186522e
commit
df3e5f2416
31
.cproject
31
.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>
|
||||
|
|
@ -316,7 +314,7 @@
|
|||
</target>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-k</buildArguments>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</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,7 +360,6 @@
|
|||
</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>
|
||||
|
|
@ -378,7 +375,6 @@
|
|||
</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 +382,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 +390,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 +397,6 @@
|
|||
</target>
|
||||
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -409,7 +404,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>
|
||||
|
|
@ -417,6 +411,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>
|
||||
|
|
@ -424,7 +419,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>
|
||||
|
|
@ -432,6 +426,7 @@
|
|||
</target>
|
||||
<target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -439,6 +434,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>
|
||||
|
|
@ -446,6 +442,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>
|
||||
|
|
@ -453,7 +450,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>
|
||||
|
|
@ -461,7 +457,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>
|
||||
|
|
@ -469,7 +464,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>
|
||||
|
|
@ -477,7 +471,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>
|
||||
|
|
@ -485,6 +478,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>
|
||||
|
|
@ -492,7 +486,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>
|
||||
|
|
@ -500,6 +493,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>
|
||||
|
|
@ -507,7 +501,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>
|
||||
|
|
@ -515,7 +508,6 @@
|
|||
</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>
|
||||
|
|
@ -523,6 +515,7 @@
|
|||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -530,6 +523,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>
|
||||
|
|
@ -537,6 +531,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>
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ namespace gtsam {
|
|||
void BayesNet<Conditional>::print(const string& s) const {
|
||||
cout << s << ":\n";
|
||||
std::string key;
|
||||
BOOST_FOREACH(conditional_ptr conditional,conditionals_)
|
||||
BOOST_FOREACH(sharedConditional conditional,conditionals_)
|
||||
conditional->print("Node[" + conditional->key() + "]");
|
||||
}
|
||||
|
||||
|
|
@ -38,26 +38,17 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
Ordering BayesNet<Conditional>::ordering() const {
|
||||
Ordering ord;
|
||||
BOOST_FOREACH(conditional_ptr conditional,conditionals_)
|
||||
BOOST_FOREACH(sharedConditional conditional,conditionals_)
|
||||
ord.push_back(conditional->key());
|
||||
return ord;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// predicate to check whether a conditional has the sought key
|
||||
template<class Conditional>
|
||||
class HasKey {
|
||||
const string& key_;
|
||||
public:
|
||||
HasKey(const std::string& key):key_(key) {}
|
||||
bool operator()(const boost::shared_ptr<Conditional>& conditional) {
|
||||
return (conditional->key()==key_);
|
||||
}
|
||||
};
|
||||
|
||||
template<class Conditional>
|
||||
boost::shared_ptr<Conditional> BayesNet<Conditional>::operator[](const std::string& key) const {
|
||||
const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),HasKey<Conditional>(key));
|
||||
typename BayesNet<Conditional>::sharedConditional
|
||||
BayesNet<Conditional>::operator[](const std::string& key) const {
|
||||
const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),onKey<Conditional>(key));
|
||||
if (it == conditionals_.end()) throw(invalid_argument(
|
||||
"BayesNet::operator['"+key+"']: not found"));
|
||||
return *it;
|
||||
|
|
|
|||
|
|
@ -31,8 +31,8 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
/** We store shared pointers to Conditional densities */
|
||||
typedef typename boost::shared_ptr<Conditional> conditional_ptr;
|
||||
typedef typename std::list<conditional_ptr> Conditionals;
|
||||
typedef typename boost::shared_ptr<Conditional> sharedConditional;
|
||||
typedef typename std::list<sharedConditional> Conditionals;
|
||||
|
||||
typedef typename Conditionals::const_iterator const_iterator;
|
||||
typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
|
||||
|
|
@ -55,12 +55,12 @@ namespace gtsam {
|
|||
bool equals(const BayesNet& other, double tol = 1e-9) const;
|
||||
|
||||
/** push_back: use reverse topological sort (i.e. parents last / elimination order) */
|
||||
inline void push_back(const boost::shared_ptr<Conditional>& conditional) {
|
||||
inline void push_back(const sharedConditional& conditional) {
|
||||
conditionals_.push_back(conditional);
|
||||
}
|
||||
|
||||
/** push_front: use topological sort (i.e. parents first / reverse elimination order) */
|
||||
inline void push_front(const boost::shared_ptr<Conditional>& conditional) {
|
||||
inline void push_front(const sharedConditional& conditional) {
|
||||
conditionals_.push_front(conditional);
|
||||
}
|
||||
|
||||
|
|
@ -73,9 +73,9 @@ namespace gtsam {
|
|||
Ordering ordering() const;
|
||||
|
||||
/** SLOW O(n) random access to Conditional by key */
|
||||
conditional_ptr operator[](const std::string& key) const;
|
||||
sharedConditional operator[](const std::string& key) const;
|
||||
|
||||
inline conditional_ptr back() { return conditionals_.back(); }
|
||||
inline sharedConditional back() { return conditionals_.back(); }
|
||||
|
||||
/** return iterators. FD: breaks encapsulation? */
|
||||
inline const_iterator const begin() const {return conditionals_.begin();}
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
BayesTree<Conditional>::Clique::Clique(const boost::shared_ptr<Conditional>& conditional) {
|
||||
BayesTree<Conditional>::Clique::Clique(const sharedConditional& conditional) {
|
||||
separator_ = conditional->parents();
|
||||
this->push_back(conditional);
|
||||
}
|
||||
|
|
@ -91,8 +91,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
void BayesTree<Conditional>::insert
|
||||
(const boost::shared_ptr<Conditional>& conditional)
|
||||
void BayesTree<Conditional>::insert(const sharedConditional& conditional)
|
||||
{
|
||||
// get key and parents
|
||||
string key = conditional->key();
|
||||
|
|
@ -129,7 +128,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
boost::shared_ptr<Conditional> BayesTree<Conditional>::marginal(const string& key) const {
|
||||
typename BayesTree<Conditional>::sharedConditional
|
||||
BayesTree<Conditional>::marginal(const string& key) const {
|
||||
|
||||
// get clique containing key, and remove all factors below key
|
||||
sharedClique clique = (*this)[key];
|
||||
|
|
@ -160,9 +160,7 @@ namespace gtsam {
|
|||
ordering.reverse();
|
||||
|
||||
// eliminate to get marginal
|
||||
boost::shared_ptr<BayesNet<Conditional> > bayesNet;
|
||||
typename boost::shared_ptr<BayesNet<Conditional> > chordalBayesNet =
|
||||
graph.eliminate(bayesNet,ordering);
|
||||
sharedBayesNet chordalBayesNet = eliminate<Factor,Conditional>(graph,ordering);
|
||||
|
||||
return chordalBayesNet->back(); // the root is the marginal
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,4 +49,17 @@ namespace gtsam {
|
|||
/** return the number of parents */
|
||||
virtual std::size_t nrParents() const = 0;
|
||||
};
|
||||
|
||||
// predicate to check whether a conditional has the sought key
|
||||
template<class Conditional>
|
||||
class onKey {
|
||||
const std::string& key_;
|
||||
public:
|
||||
onKey(const std::string& key):key_(key) {}
|
||||
bool operator()(const typename Conditional::shared_ptr& conditional) {
|
||||
return (conditional->key()==key_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -83,7 +83,8 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order
|
|||
}
|
||||
else
|
||||
{
|
||||
ConditionalGaussian::shared_ptr cg = eliminateOne<ConditionalGaussian>(key);
|
||||
ConditionalGaussian::shared_ptr cg =
|
||||
eliminateOne<LinearFactor,ConditionalGaussian>(*this,key);
|
||||
cbn->push_back(cg);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet)
|
|||
{
|
||||
typename BayesNet<Conditional>::const_iterator it = bayesNet.begin();
|
||||
for(; it != bayesNet.end(); it++) {
|
||||
typename boost::shared_ptr<Factor>::shared_ptr factor(new Factor(*it));
|
||||
sharedFactor factor(new Factor(*it));
|
||||
push_back(factor);
|
||||
}
|
||||
}
|
||||
|
|
@ -56,7 +56,7 @@ bool FactorGraph<Factor>::equals
|
|||
/** check whether the factors_ are the same */
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
// TODO: Doesn't this force order of factor insertion?
|
||||
shared_factor f1 = factors_[i], f2 = fg.factors_[i];
|
||||
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
|
||||
if (f1 == NULL && f2 == NULL) continue;
|
||||
if (f1 == NULL || f2 == NULL) return false;
|
||||
if (!f1->equals(*f2, tol)) return false;
|
||||
|
|
@ -75,7 +75,7 @@ size_t FactorGraph<Factor>::nrFactors() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
void FactorGraph<Factor>::push_back(shared_factor factor) {
|
||||
void FactorGraph<Factor>::push_back(sharedFactor factor) {
|
||||
factors_.push_back(factor); // add the actual factor
|
||||
if (factor==NULL) return;
|
||||
|
||||
|
|
@ -184,11 +184,11 @@ list<int> FactorGraph<Factor>::factors(const string& key) const {
|
|||
template<class Factor>
|
||||
vector<boost::shared_ptr<Factor> >
|
||||
FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
|
||||
vector<boost::shared_ptr<Factor> > found;
|
||||
vector<sharedFactor> found;
|
||||
|
||||
Indices::iterator it = indices_.find(key);
|
||||
if (it == indices_.end())
|
||||
throw(std::invalid_argument
|
||||
throw(invalid_argument
|
||||
("FactorGraph::findAndRemoveFactors invalid key: " + key));
|
||||
|
||||
list<int> *indices_ptr; // pointer to indices list in indices_ map
|
||||
|
|
@ -206,14 +206,13 @@ FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
|
|||
/* find factors and remove them from the factor graph: O(n) */
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
boost::shared_ptr<Factor>
|
||||
typename FactorGraph<Factor>::sharedFactor
|
||||
FactorGraph<Factor>::removeAndCombineFactors(const string& key)
|
||||
{
|
||||
bool verbose = false;
|
||||
if (verbose) cout << "FactorGraph::removeAndCombineFactors" << endl;
|
||||
typedef typename boost::shared_ptr<Factor> shared_factor;
|
||||
vector<shared_factor> found = findAndRemoveFactors(key);
|
||||
shared_factor new_factor(new Factor(found));
|
||||
vector<sharedFactor> found = findAndRemoveFactors(key);
|
||||
sharedFactor new_factor(new Factor(found));
|
||||
if (verbose) cout << "FactorGraph::removeAndCombineFactors done" << endl;
|
||||
return new_factor;
|
||||
}
|
||||
|
|
@ -221,21 +220,20 @@ FactorGraph<Factor>::removeAndCombineFactors(const string& key)
|
|||
/* ************************************************************************* */
|
||||
/* eliminate one node from the factor graph */
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
template<class Conditional>
|
||||
boost::shared_ptr<Conditional> FactorGraph<Factor>::eliminateOne(const std::string& key) {
|
||||
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
|
||||
shared_factor joint_factor = removeAndCombineFactors(key);
|
||||
boost::shared_ptr<Factor> joint_factor = graph.removeAndCombineFactors(key);
|
||||
|
||||
// eliminate that joint factor
|
||||
shared_factor 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()) push_back(factor);
|
||||
if (!factor->empty()) graph.push_back(factor);
|
||||
|
||||
// return the conditional Gaussian
|
||||
return conditional;
|
||||
|
|
@ -245,16 +243,16 @@ boost::shared_ptr<Conditional> FactorGraph<Factor>::eliminateOne(const std::stri
|
|||
// 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>
|
||||
template<class Conditional>
|
||||
template<class Factor,class Conditional>
|
||||
boost::shared_ptr<BayesNet<Conditional> >
|
||||
FactorGraph<Factor>::eliminate(boost::shared_ptr<BayesNet<Conditional> > xxx, const Ordering& ordering)
|
||||
eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering)
|
||||
{
|
||||
boost::shared_ptr<BayesNet<Conditional> > bayesNet (new BayesNet<Conditional>()); // empty
|
||||
|
||||
BOOST_FOREACH(string key, ordering) {
|
||||
boost::shared_ptr<Conditional> cg = eliminateOne<Conditional>(key);
|
||||
boost::shared_ptr<Conditional> cg = eliminateOne<Factor,Conditional>(factorGraph,key);
|
||||
bayesNet->push_back(cg);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,13 +30,13 @@ namespace gtsam {
|
|||
*/
|
||||
template<class Factor> class FactorGraph: public Testable<FactorGraph<Factor> > {
|
||||
public:
|
||||
typedef typename boost::shared_ptr<Factor> shared_factor;
|
||||
typedef typename std::vector<shared_factor>::iterator iterator;
|
||||
typedef typename std::vector<shared_factor>::const_iterator const_iterator;
|
||||
typedef typename boost::shared_ptr<Factor> sharedFactor;
|
||||
typedef typename std::vector<sharedFactor>::iterator iterator;
|
||||
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
|
||||
|
||||
protected:
|
||||
/** Collection of factors */
|
||||
std::vector<shared_factor> factors_;
|
||||
std::vector<sharedFactor> factors_;
|
||||
|
||||
/** For each variable a list of factor indices connected to it */
|
||||
typedef std::map<std::string, std::list<int> > Indices;
|
||||
|
|
@ -73,7 +73,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Get a specific factor by index */
|
||||
inline shared_factor operator[](size_t i) const {
|
||||
inline sharedFactor operator[](size_t i) const {
|
||||
return factors_[i];
|
||||
}
|
||||
|
||||
|
|
@ -84,7 +84,7 @@ namespace gtsam {
|
|||
size_t nrFactors() const;
|
||||
|
||||
/** Add a factor */
|
||||
void push_back(shared_factor factor);
|
||||
void push_back(sharedFactor factor);
|
||||
|
||||
/**
|
||||
* Compute colamd ordering
|
||||
|
|
@ -102,29 +102,14 @@ namespace gtsam {
|
|||
* from the factor graph
|
||||
* @param key the key for the given node
|
||||
*/
|
||||
std::vector<shared_factor> findAndRemoveFactors(const std::string& key);
|
||||
std::vector<sharedFactor> findAndRemoveFactors(const std::string& key);
|
||||
|
||||
/**
|
||||
* extract and combine all the factors that involve a given node
|
||||
* @param key the key for the given node
|
||||
* @return the combined linear factor
|
||||
*/
|
||||
shared_factor removeAndCombineFactors(const std::string& key);
|
||||
|
||||
/**
|
||||
* 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 Conditional>
|
||||
boost::shared_ptr<Conditional> eliminateOne(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 Conditional>
|
||||
boost::shared_ptr<BayesNet<Conditional> > eliminate(boost::shared_ptr<BayesNet<Conditional> > bayesNet, const Ordering& ordering);
|
||||
sharedFactor removeAndCombineFactors(const std::string& key);
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -136,5 +121,24 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(indices_);
|
||||
}
|
||||
}; // FactorGraph
|
||||
|
||||
/** 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>
|
||||
boost::shared_ptr<BayesNet<Conditional> >
|
||||
eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering);
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ LinearFactorGraph::LinearFactorGraph(const GaussianBayesNet& CBN) :
|
|||
set<string> LinearFactorGraph::find_separator(const string& key) const
|
||||
{
|
||||
set<string> separator;
|
||||
BOOST_FOREACH(shared_factor factor,factors_)
|
||||
BOOST_FOREACH(sharedFactor factor,factors_)
|
||||
factor->tally_separator(key,separator);
|
||||
|
||||
return separator;
|
||||
|
|
@ -46,7 +46,8 @@ LinearFactorGraph::eliminate(const Ordering& ordering)
|
|||
{
|
||||
GaussianBayesNet::shared_ptr chordalBayesNet (new GaussianBayesNet()); // empty
|
||||
BOOST_FOREACH(string key, ordering) {
|
||||
ConditionalGaussian::shared_ptr cg = eliminateOne<ConditionalGaussian>(key);
|
||||
ConditionalGaussian::shared_ptr cg =
|
||||
eliminateOne<LinearFactor,ConditionalGaussian>(*this, key);
|
||||
chordalBayesNet->push_back(cg);
|
||||
}
|
||||
return chordalBayesNet;
|
||||
|
|
@ -90,7 +91,7 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
|
|||
/* ************************************************************************* */
|
||||
Dimensions LinearFactorGraph::dimensions() const {
|
||||
Dimensions result;
|
||||
BOOST_FOREACH(shared_factor factor,factors_) {
|
||||
BOOST_FOREACH(sharedFactor factor,factors_) {
|
||||
Dimensions vs = factor->dimensions();
|
||||
string key; int dim;
|
||||
FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim));
|
||||
|
|
@ -112,7 +113,7 @@ LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const {
|
|||
FOREACH_PAIR(key,dim,vs) {
|
||||
Matrix A = eye(dim);
|
||||
Vector b = zero(dim);
|
||||
shared_factor prior(new LinearFactor(key,A,b, sigma));
|
||||
sharedFactor prior(new LinearFactor(key,A,b, sigma));
|
||||
result.push_back(prior);
|
||||
}
|
||||
return result;
|
||||
|
|
@ -123,7 +124,7 @@ pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
|
|||
|
||||
// get all factors
|
||||
LinearFactorSet found;
|
||||
BOOST_FOREACH(shared_factor factor,factors_)
|
||||
BOOST_FOREACH(sharedFactor factor,factors_)
|
||||
found.push_back(factor);
|
||||
|
||||
// combine them
|
||||
|
|
@ -145,7 +146,7 @@ Matrix LinearFactorGraph::sparse(const Ordering& ordering) const {
|
|||
|
||||
// Collect the I,J,S lists for all factors
|
||||
int row_index = 0;
|
||||
BOOST_FOREACH(shared_factor factor,factors_) {
|
||||
BOOST_FOREACH(sharedFactor factor,factors_) {
|
||||
|
||||
// get sparse lists for the factor
|
||||
list<int> i1,j1;
|
||||
|
|
|
|||
|
|
@ -25,7 +25,8 @@ namespace gtsam {
|
|||
SymbolicBayesNet::shared_ptr bayesNet (new SymbolicBayesNet());
|
||||
|
||||
BOOST_FOREACH(string key, ordering) {
|
||||
SymbolicConditional::shared_ptr conditional = eliminateOne<SymbolicConditional>(key);
|
||||
SymbolicConditional::shared_ptr conditional =
|
||||
eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key);
|
||||
bayesNet->push_back(conditional);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -256,7 +256,8 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
|
|||
CHECK(fg.nrFactors() == 0);
|
||||
|
||||
// eliminate the linear factor
|
||||
ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne<ConditionalGaussian>("z");
|
||||
ConditionalGaussian::shared_ptr cg3 =
|
||||
eliminateOne<LinearFactor,ConditionalGaussian>(fg,"z");
|
||||
CHECK(cg3->nrParents() == 0);
|
||||
CHECK(fg.size() == 0);
|
||||
|
||||
|
|
|
|||
|
|
@ -191,7 +191,7 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
|
|||
{
|
||||
LinearFactorGraph fg = createLinearFactorGraph();
|
||||
ConditionalGaussian::shared_ptr actual =
|
||||
fg.eliminateOne<ConditionalGaussian>("x1");
|
||||
eliminateOne<LinearFactor,ConditionalGaussian>(fg,"x1");
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
Matrix R11 = Matrix_(2,2,
|
||||
|
|
@ -220,7 +220,7 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
|
|||
{
|
||||
LinearFactorGraph fg = createLinearFactorGraph();
|
||||
ConditionalGaussian::shared_ptr actual =
|
||||
fg.eliminateOne<ConditionalGaussian>("x2");
|
||||
eliminateOne<LinearFactor,ConditionalGaussian>(fg,"x2");
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
Matrix R11 = Matrix_(2,2,
|
||||
|
|
@ -248,7 +248,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
|
|||
{
|
||||
LinearFactorGraph fg = createLinearFactorGraph();
|
||||
ConditionalGaussian::shared_ptr actual =
|
||||
fg.eliminateOne<ConditionalGaussian>("l1");
|
||||
eliminateOne<LinearFactor,ConditionalGaussian>(fg,"l1");
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
Matrix R11 = Matrix_(2,2,
|
||||
|
|
@ -383,8 +383,8 @@ TEST( LinearFactorGraph, matrix )
|
|||
);
|
||||
Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5);
|
||||
|
||||
EQUALITY(A,A1); // currently fails
|
||||
CHECK(b==b1); // currently fails
|
||||
EQUALITY(A,A1);
|
||||
CHECK(b==b1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -400,9 +400,10 @@ TEST( LinearFactorGraph, sparse )
|
|||
Matrix ijs = fg.sparse(ord);
|
||||
|
||||
EQUALITY(ijs, Matrix_(3, 14,
|
||||
+1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8.,7.,8.,
|
||||
+5., 6., 1., 2., 5., 6., 3.,4., 5., 6., 1., 2.,3.,4.,
|
||||
10.,10., 10.,10.,-10.,-10., 5.,5.,-5.,-5., -5.,-5.,5.,5.));
|
||||
// f(x1) f(x2,x1) f(l1,x1) f(x2,l1)
|
||||
+1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8.,7.,8.,
|
||||
+5., 6., 1., 2., 5., 6., 3.,4., 5., 6., 1., 2.,3.,4.,
|
||||
10.,10., 10.,10.,-10.,-10., 5.,5.,-5.,-5., -5.,-5.,5.,5.));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -422,9 +423,8 @@ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet )
|
|||
|
||||
// Base FactorGraph only
|
||||
FactorGraph<LinearFactor> fg3(*CBN);
|
||||
boost::shared_ptr<BayesNet<ConditionalGaussian> > dummy;
|
||||
boost::shared_ptr<BayesNet<ConditionalGaussian> > CBN3 =
|
||||
fg3.eliminate<ConditionalGaussian>(dummy,ord);
|
||||
eliminate<LinearFactor,ConditionalGaussian>(fg3,ord);
|
||||
CHECK(CBN->equals(*CBN3));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -112,7 +112,7 @@ TEST( LinearFactorGraph, eliminateOne )
|
|||
|
||||
// eliminate
|
||||
SymbolicConditional::shared_ptr actual =
|
||||
fg.eliminateOne<SymbolicConditional>("x1");
|
||||
eliminateOne<SymbolicFactor,SymbolicConditional>(fg,"x1");
|
||||
|
||||
// create expected symbolic Conditional
|
||||
SymbolicConditional expected("x1","l1","x2");
|
||||
|
|
|
|||
Loading…
Reference in New Issue