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

Small change necessitating lots of edits: Conditionals now include key of random variable 

This simplifies Bayes nets quite a bit. Also created a Conditional base class, derived classes ConditionalGaussian and SymbolicConditional
Finally, some changes were needed because I moved some headers to .cpp
release/4.3a0
Frank Dellaert 2009-11-02 03:50:30 +00:00
parent 943b692a6b
commit a8d267c4ca
33 changed files with 566 additions and 485 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
.cproject
View File

@ -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,6 +314,7 @@
</target>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -323,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>
@ -339,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>
@ -346,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>
@ -354,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>
@ -361,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>
@ -377,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>
@ -385,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>
@ -392,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>
@ -400,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>
@ -408,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>
@ -416,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>
@ -423,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>
@ -431,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>
@ -438,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>
@ -445,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>
@ -452,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>
@ -460,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>
@ -468,22 +464,21 @@
</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>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testChordalBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testChordalBayesNet.run</buildTarget>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
@ -491,6 +486,7 @@
</target>
<target name="testSymbolicBayesChain.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesChain.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -498,6 +494,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>
@ -505,7 +502,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>
@ -513,7 +509,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>
@ -521,6 +516,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>
@ -528,6 +524,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>
@ -535,6 +532,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>

46
cpp/BayesNet-inl.h
View File

@ -8,49 +8,42 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/vector.hpp> // for +=
using namespace boost::assign;
#include "Ordering.h"
#include "BayesNet.h"
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesNet<Conditional>::print(const string& s) const {
cout << s << ":\n";
BOOST_FOREACH(string key, keys_) {
const_iterator it = nodes_.find(key);
it->second->print("Node[" + key + "]");
}
std::string key;
BOOST_FOREACH(conditional_ptr conditional,conditionals_)
conditional->print("Node[" + conditional->key() + "]");
}
/* ************************************************************************* */
template<class Conditional>
bool BayesNet<Conditional>::equals(const BayesNet& cbn, double tol) const {
if(indices_ != cbn.indices_) return false;
if(size() != cbn.size()) return false;
if(keys_ != cbn.keys_) return false;
string key;
boost::shared_ptr<Conditional> node;
FOREACH_PAIR( key, node, nodes_) {
const_iterator cg = cbn.nodes_.find(key);
if (cg == nodes_.end()) return false;
if (!equals_star(node,cg->second,tol)) return false;
}
return true;
return equal(conditionals_.begin(),conditionals_.begin(),conditionals_.begin(),equals_star<Conditional>);
}
/* ************************************************************************* */
template<class Conditional>
void BayesNet<Conditional>::insert
(const string& key, boost::shared_ptr<Conditional> node) {
keys_.push_front(key);
nodes_.insert(make_pair(key,node));
(const boost::shared_ptr<Conditional>& conditional) {
indices_.insert(make_pair(conditional->key(),conditionals_.size()));
conditionals_.push_back(conditional);
}
/* ************************************************************************* */
/* ************************************************************************* *
template<class Conditional>
void BayesNet<Conditional>::erase(const string& key) {
list<string>::iterator it;
@ -59,9 +52,18 @@ namespace gtsam {
break;
}
keys_.erase(it);
nodes_.erase(key);
conditionals_.erase(key);
}
/* ************************************************************************* */
/* ************************************************************************* */
template<class Conditional>
Ordering BayesNet<Conditional>::ordering() const {
Ordering ord;
BOOST_FOREACH(conditional_ptr conditional,conditionals_)
ord.push_back(conditional->key());
return ord;
}
/* ************************************************************************* */
} // namespace gtsam

81
cpp/BayesNet.h
View File

@ -8,14 +8,18 @@
#pragma once
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include "Testable.h"
namespace gtsam {
class Ordering;
/**
* Bayes network
* This is the base class for SymbolicBayesNet, DiscreteBayesNet, and GaussianBayesNet
@ -24,14 +28,36 @@ namespace gtsam {
*/
template<class Conditional>
class BayesNet: public Testable<BayesNet<Conditional> > {
public:
/** We store shared pointers to Conditional densities */
typedef typename boost::shared_ptr<Conditional> conditional_ptr;
typedef typename std::vector<conditional_ptr> Conditionals;
typedef typename Conditionals::const_iterator const_iterator;
typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
protected:
/** nodes keys stored in topological sort order, i.e. from parents to children */
std::list<std::string> keys_;
/**
* Conditional densities are stored in reverse topological sort order (i.e., leaves first,
* parents last), which corresponds to the elimination ordering if so obtained,
* and is consistent with the column (block) ordering of an upper triangular matrix.
*/
Conditionals conditionals_;
/** nodes stored on key */
typedef typename std::map<std::string, boost::shared_ptr<Conditional> > Nodes;
Nodes nodes_;
/**
* O(log n) random access on keys will provided by a map from keys to vector index.
*/
typedef std::map<std::string, int> Indices;
Indices indices_;
/** O(log n) lookup from key to node index */
inline int index(const std::string& key) const {
Indices::const_iterator it = indices_.find(key); // get node index
assert( it != indices_.end() );
return it->second;
}
public:
@ -41,37 +67,36 @@ namespace gtsam {
/** check equality */
bool equals(const BayesNet& other, double tol = 1e-9) const;
/** insert: use reverse topological sort (i.e. parents last) */
void insert(const std::string& key, boost::shared_ptr<Conditional> node);
/** delete */
void erase(const std::string& key);
/** insert: use reverse topological sort (i.e. parents last / elimination order) */
void insert(const boost::shared_ptr<Conditional>& conditional);
/** size is the number of nodes */
inline size_t size() const {return nodes_.size();}
/** return keys in topological sort order (parents first), i.e., reverse elimination order */
inline std::list<std::string> keys() const { return keys_;}
inline boost::shared_ptr<Conditional> operator[](const std::string& key) const {
const_iterator cg = nodes_.find(key); // get node
assert( cg != nodes_.end() );
return cg->second;
inline size_t size() const {
return conditionals_.size();
}
/** return begin and end of the nodes. FD: breaks encapsulation? */
typedef typename Nodes::const_iterator const_iterator;
const_iterator const begin() const {return nodes_.begin();}
const_iterator const end() const {return nodes_.end();}
/** return keys in reverse topological sort order, i.e., elimination order */
Ordering ordering() const;
/** O(log n) random access to Conditional by key */
inline conditional_ptr operator[](const std::string& key) const {
int i = index(key);
return conditionals_[i];
}
/** return iterators. FD: breaks encapsulation? */
const_iterator const begin() const {return conditionals_.begin();}
const_iterator const end() const {return conditionals_.end();}
const_reverse_iterator const rbegin() const {return conditionals_.rbegin();}
const_reverse_iterator const rend() const {return conditionals_.rend();}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(keys_);
ar & BOOST_SERIALIZATION_NVP(nodes_);
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_);
ar & BOOST_SERIALIZATION_NVP(indices_);
}
};

38
cpp/BayesTree-inl.h
View File

@ -13,8 +13,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
Front<Conditional>::Front(string key, cond_ptr conditional) {
add(key, conditional);
Front<Conditional>::Front(const conditional_ptr& conditional) {
add(conditional);
separator_ = conditional->parents();
}
@ -22,11 +22,12 @@ namespace gtsam {
template<class Conditional>
void Front<Conditional>::print(const string& s) const {
cout << s;
BOOST_FOREACH(string key, keys_) cout << " " << key;
BOOST_FOREACH(const conditional_ptr& conditional, conditionals_)
cout << " " << conditional->key();
if (!separator_.empty()) {
cout << " :";
BOOST_FOREACH(string key, separator_)
cout << " " << key;
cout << " " << key;
}
cout << endl;
}
@ -34,14 +35,12 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
bool Front<Conditional>::equals(const Front<Conditional>& other, double tol) const {
return (keys_ == other.keys_) &&
equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star<Conditional>);
return equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star<Conditional>);
}
/* ************************************************************************* */
template<class Conditional>
void Front<Conditional>::add(string key, cond_ptr conditional) {
keys_.push_front(key);
void Front<Conditional>::add(const conditional_ptr& conditional) {
conditionals_.push_front(conditional);
}
@ -51,12 +50,13 @@ namespace gtsam {
}
/* ************************************************************************* */
// TODO: traversal is O(n*log(n)) but could be O(n) with better bayesChain
// TODO: traversal is O(n*log(n)) but could be O(n) with better bayesNet
template<class Conditional>
BayesTree<Conditional>::BayesTree(BayesNet<Conditional>& bayesChain, bool verbose) {
list<string> reverseOrdering = bayesChain.keys();
BOOST_FOREACH(string key, reverseOrdering)
insert(key,bayesChain[key],verbose);
BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet, bool verbose) {
typename BayesNet<Conditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit < bayesNet.rend(); ++rit ) {
insert(*rit,verbose);
}
}
/* ************************************************************************* */
@ -78,8 +78,9 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::insert(string key, conditional_ptr conditional, bool verbose) {
void BayesTree<Conditional>::insert(const boost::shared_ptr<Conditional>& conditional, bool verbose) {
string key = conditional->key();
if (verbose) cout << "Inserting " << key << "| ";
// get parents
@ -90,7 +91,7 @@ namespace gtsam {
// if no parents, start a new root clique
if (parents.empty()) {
if (verbose) cout << "Creating root clique" << endl;
node_ptr root(new Node(key, conditional));
node_ptr root(new Node(conditional));
nodes_.push_back(root);
nodeMap_.insert(make_pair(key, 0));
return;
@ -108,13 +109,13 @@ namespace gtsam {
if (parent_clique->size() == parents.size()) {
if (verbose) cout << "Adding to clique " << index << endl;
nodeMap_.insert(make_pair(key, index));
parent_clique->add(key, conditional);
parent_clique->add(conditional);
return;
}
// otherwise, start a new clique and add it to the tree
if (verbose) cout << "Starting new clique" << endl;
node_ptr new_clique(new Node(key, conditional));
node_ptr new_clique(new Node(conditional));
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
nodeMap_.insert(make_pair(key, nodes_.size()));
@ -123,4 +124,5 @@ namespace gtsam {
/* ************************************************************************* */
} /// namespace gtsam
}
/// namespace gtsam

22
cpp/BayesTree.h
View File

@ -22,14 +22,13 @@ namespace gtsam {
template<class Conditional>
class Front: Testable<Front<Conditional> > {
private:
typedef boost::shared_ptr<Conditional> cond_ptr;
std::list<std::string> keys_; /** frontal keys */
std::list<cond_ptr> conditionals_; /** conditionals */
typedef boost::shared_ptr<Conditional> conditional_ptr;
std::list<conditional_ptr> conditionals_; /** conditionals */
std::list<std::string> separator_; /** separator keys */
public:
/** constructor */
Front(std::string key, cond_ptr conditional);
Front(const conditional_ptr& conditional);
/** print */
void print(const std::string& s = "") const;
@ -38,10 +37,10 @@ namespace gtsam {
bool equals(const Front<Conditional>& other, double tol = 1e-9) const;
/** add a frontal node */
void add(std::string key, cond_ptr conditional);
void add(const conditional_ptr& conditional);
/** return size of the clique */
inline size_t size() const {return keys_.size() + separator_.size();}
inline size_t size() const {return conditionals_.size() + separator_.size();}
};
/**
@ -55,6 +54,7 @@ namespace gtsam {
public:
typedef boost::shared_ptr<Conditional> conditional_ptr;
typedef std::pair<std::string,conditional_ptr> NamedConditional;
private:
@ -64,7 +64,7 @@ namespace gtsam {
shared_ptr parent_;
std::list<shared_ptr> children_;
Node(std::string key, conditional_ptr conditional):Front<Conditional>(key,conditional) {}
Node(const boost::shared_ptr<Conditional>& conditional):Front<Conditional>(conditional) {}
/** print this node and entire subtree below it*/
void printTree(const std::string& indent) const {
@ -88,8 +88,8 @@ namespace gtsam {
/** Create an empty Bayes Tree */
BayesTree();
/** Create a Bayes Tree from a SymbolicBayesNet */
BayesTree(BayesNet<Conditional>& bayesChain, bool verbose=false);
/** Create a Bayes Tree from a Bayes Net */
BayesTree(const BayesNet<Conditional>& bayesNet, bool verbose=false);
/** Destructor */
virtual ~BayesTree() {}
@ -101,9 +101,9 @@ namespace gtsam {
bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
/** insert a new conditional */
void insert(std::string key, conditional_ptr conditional, bool verbose=false);
void insert(const boost::shared_ptr<Conditional>& conditional, bool verbose=false);
/** number of cliques */
/** number of cliques */
inline size_t size() const { return nodes_.size();}
/** return root clique */

52
cpp/Conditional.h Normal file
View File

@ -0,0 +1,52 @@
/**
* @file Conditional.h
* @brief Base class for conditional densities
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <boost/utility.hpp> // for noncopyable
#include "Testable.h"
namespace gtsam {
/**
* Base class for conditional densities
*
* We make it noncopyable so we enforce the fact that factors are
* kept in pointer containers. To be safe, you should make them
* immutable, i.e., practicing functional programming.
*/
class Conditional : boost::noncopyable, public Testable<Conditional>
{
protected:
/** key of random variable */
std::string key_;
public:
/** constructor */
Conditional(const std::string& key):key_(key) {}
/* destructor */
virtual ~Conditional() {};
/** check equality */
bool equals(const Conditional& c, double tol = 1e-9) const {
return key_ == c.key_;
}
/** return key */
inline const std::string& key() const { return key_;}
/** return parent keys */
virtual std::list<std::string> parents() const = 0;
/** return the number of parents */
virtual std::size_t nrParents() const = 0;
};
}

57
cpp/ConditionalGaussian.cpp
View File

@ -4,49 +4,39 @@
* @author Christian Potthast
*/
#include <string.h>
#include <boost/numeric/ublas/vector.hpp>
#include "Ordering.h"
#include "ConditionalGaussian.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(Vector d,Matrix R) : R_(R),d_(d)
{
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R) :
Conditional (key), R_(R), d_(d) {
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(Vector d,
Matrix R,
const string& name1,
Matrix S)
: R_(R),d_(d)
{
parents_.insert(make_pair(name1, S));
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
const string& name1, Matrix S) :
Conditional (key), R_(R), d_(d) {
parents_.insert(make_pair(name1, S));
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(Vector d,
Matrix R,
const string& name1,
Matrix S,
const string& name2,
Matrix T)
: R_(R),d_(d)
{
parents_.insert(make_pair(name1, S));
parents_.insert(make_pair(name2, T));
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T) :
Conditional (key), R_(R), d_(d) {
parents_.insert(make_pair(name1, S));
parents_.insert(make_pair(name2, T));
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const Vector& d,
const Matrix& R,
const map<string, Matrix>& parents)
: R_(R), d_(d), parents_(parents)
{
}
ConditionalGaussian::ConditionalGaussian(const string& key,
const Vector& d, const Matrix& R, const map<string, Matrix>& parents) :
Conditional (key), R_(R), d_(d), parents_(parents) {
}
/* ************************************************************************* */
void ConditionalGaussian::print(const string &s) const
@ -62,23 +52,26 @@ void ConditionalGaussian::print(const string &s) const
}
/* ************************************************************************* */
bool ConditionalGaussian::equals(const ConditionalGaussian &cg, double tol) const {
bool ConditionalGaussian::equals(const Conditional &c, double tol) const {
if (!Conditional::equals(c)) return false;
const ConditionalGaussian* p = dynamic_cast<const ConditionalGaussian*> (&c);
if (p == NULL) return false;
Parents::const_iterator it = parents_.begin();
// check if the size of the parents_ map is the same
if (parents_.size() != cg.parents_.size()) return false;
if (parents_.size() != p->parents_.size()) return false;
// check if R_ is equal
if (!(equal_with_abs_tol(R_, cg.R_, tol))) return false;
if (!(equal_with_abs_tol(R_, p->R_, tol))) return false;
// check if d_ is equal
if (!(::equal_with_abs_tol(d_, cg.d_, tol))) return false;
if (!(::equal_with_abs_tol(d_, p->d_, tol))) return false;
// check if the matrices are the same
// iterate over the parents_ map
for (it = parents_.begin(); it != parents_.end(); it++) {
Parents::const_iterator it2 = cg.parents_.find(it->first.c_str());
if (it2 != cg.parents_.end()) {
Parents::const_iterator it2 = p->parents_.find(it->first.c_str());
if (it2 != p->parents_.end()) {
if (!(equal_with_abs_tol(it->second, it2->second, tol))) return false;
} else
return false;

196
cpp/ConditionalGaussian.h
View File

@ -4,7 +4,6 @@
* @author Christian Potthast
*/
// \callgraph
#pragma once
@ -14,128 +13,131 @@
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/string.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include "Matrix.h"
#include "Conditional.h"
#include "VectorConfig.h"
#include "Ordering.h"
#include "Testable.h"
#include "Matrix.h"
namespace gtsam {
/**
* A conditional Gaussian functions as the node in a Bayes network
* It has a set of parents y,z, etc. and implements a probability density on x.
* The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
*/
class ConditionalGaussian : boost::noncopyable, public Testable<ConditionalGaussian>
{
public:
typedef std::map<std::string, Matrix> Parents;
typedef Parents::const_iterator const_iterator;
typedef boost::shared_ptr<ConditionalGaussian> shared_ptr;
protected:
/** the triangular matrix (square root information matrix) */
Matrix R_;
class Ordering;
/** the names and the matrices connecting to parent nodes */
Parents parents_;
/**
* A conditional Gaussian functions as the node in a Bayes network
* It has a set of parents y,z, etc. and implements a probability density on x.
* The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
*/
class ConditionalGaussian: public Conditional {
public:
typedef std::map<std::string, Matrix> Parents;
typedef Parents::const_iterator const_iterator;
typedef boost::shared_ptr<ConditionalGaussian> shared_ptr;
/** the RHS vector */
Vector d_;
protected:
public:
/** the triangular matrix (square root information matrix) */
Matrix R_;
/** constructor */
ConditionalGaussian() {};
/** Copy Constructor */
ConditionalGaussian(const ConditionalGaussian &cg) :
boost::noncopyable(), R_(cg.R_), parents_(cg.parents_), d_(cg.d_){}
/** constructor with no parents
* |Rx-d|
*/
ConditionalGaussian(Vector d,
Matrix R);
/** constructor with only one parent
* |Rx+Sy-d|
*/
ConditionalGaussian(Vector d,
Matrix R,
const std::string& name1,
Matrix S
);
/** constructor with two parents
* |Rx+Sy+Tz-d|
*/
ConditionalGaussian(Vector d,
Matrix R,
const std::string& name1,
Matrix S,
const std::string& name2,
Matrix T
);
/** the names and the matrices connecting to parent nodes */
Parents parents_;
/**
* constructor with number of arbitrary parents
* |Rx+sum(Ai*xi)-d|
*/
ConditionalGaussian(const Vector& d,
const Matrix& R,
const Parents& parents);
/** the RHS vector */
Vector d_;
/** deconstructor */
virtual ~ConditionalGaussian() {};
public:
/** print */
void print(const std::string& = "ConditionalGaussian") const;
/** default constructor needed for serialization */
ConditionalGaussian():Conditional("__unitialized__") {}
/** equals function */
bool equals(const ConditionalGaussian &cg, double tol=1e-9) const;
/** constructor */
ConditionalGaussian(const std::string& key) :
Conditional (key) {}
/** dimension of multivariate variable */
size_t dim() const {return R_.size2();}
/** constructor with no parents
* |Rx-d|
*/
ConditionalGaussian(const std::string& key, Vector d, Matrix R);
/** return all parents */
std::list<std::string> parents() const;
/** constructor with only one parent
* |Rx+Sy-d|
*/
ConditionalGaussian(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S);
/** return stuff contained in ConditionalGaussian */
const Vector& get_d() const {return d_;}
const Matrix& get_R() const {return R_;}
/** constructor with two parents
* |Rx+Sy+Tz-d|
*/
ConditionalGaussian(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T);
/** STL like, return the iterator pointing to the first node */
const_iterator const parentsBegin() const { return parents_.begin(); }
/**
* constructor with number of arbitrary parents
* |Rx+sum(Ai*xi)-d|
*/
ConditionalGaussian(const std::string& key, const Vector& d,
const Matrix& R, const Parents& parents);
/** STL like, return the iterator pointing to the last node */
const_iterator const parentsEnd() const { return parents_.end(); }
/** deconstructor */
virtual ~ConditionalGaussian() {}
/** find the number of parents */
size_t size() const {return parents_.size();}
/** print */
void print(const std::string& = "ConditionalGaussian") const;
/** determine whether a key is among the parents */
size_t contains(const std::string& key) const {return parents_.count(key);}
/** equals function */
bool equals(const Conditional &cg, double tol = 1e-9) const;
/**
* solve a conditional Gaussian
* @param x configuration in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...)
*/
virtual Vector solve(const VectorConfig& x) const;
/** dimension of multivariate variable */
size_t dim() const { return R_.size2();}
/**
* adds a parent
*/
void add(const std::string key, Matrix S){ parents_.insert(make_pair(key, S)); }
private:
/** return all parents */
std::list<std::string> parents() const;
/** return stuff contained in ConditionalGaussian */
const Vector& get_d() const { return d_;}
const Matrix& get_R() const { return R_;}
/** STL like, return the iterator pointing to the first node */
const_iterator const parentsBegin() const {
return parents_.begin();
}
/** STL like, return the iterator pointing to the last node */
const_iterator const parentsEnd() const {
return parents_.end();
}
/** find the number of parents */
size_t nrParents() const {
return parents_.size();
}
/** determine whether a key is among the parents */
size_t contains(const std::string& key) const {
return parents_.count(key);
}
/**
* solve a conditional Gaussian
* @param x configuration in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...)
*/
virtual Vector solve(const VectorConfig& x) const;
/**
* adds a parent
*/
void add(const std::string key, Matrix S) {
parents_.insert(make_pair(key, S));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(key_);
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(d_);
ar & BOOST_SERIALIZATION_NVP(parents_);

59
cpp/ConstrainedConditionalGaussian.cpp
View File

@ -12,40 +12,39 @@
using namespace gtsam;
using namespace std;
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian() {
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key) :
ConditionalGaussian(key) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& v) :
ConditionalGaussian(v, eye(v.size())) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b,
const Matrix& A) :
ConditionalGaussian(b, A) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b,
const Matrix& A1, const std::string& parent, const Matrix& A2) :
ConditionalGaussian(b, A1, parent, A2) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b,
const Matrix& A1, const std::string& parentY, const Matrix& A2,
const std::string& parentZ, const Matrix& A3)
: ConditionalGaussian(b, A1, parentY, A2, parentZ, A3)
{
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Matrix& A1,
const std::map<std::string, Matrix>& parents, const Vector& b)
: ConditionalGaussian(b, A1, parents)
{
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const ConstrainedConditionalGaussian& df) {
const string& key, const Vector& v) :
ConditionalGaussian(key, v, eye(v.size())) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A) :
ConditionalGaussian(key, b, A) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A1,
const std::string& parent, const Matrix& A2) :
ConditionalGaussian(key, b, A1, parent, A2) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A1,
const std::string& parentY, const Matrix& A2, const std::string& parentZ,
const Matrix& A3) :
ConditionalGaussian(key, b, A1, parentY, A2, parentZ, A3) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Matrix& A1,
const std::map<std::string, Matrix>& parents, const Vector& b) :
ConditionalGaussian(key, b, A1, parents) {
}
Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const {

17
cpp/ConstrainedConditionalGaussian.h
View File

@ -33,14 +33,14 @@ public:
* Default Constructor
* Don't use this
*/
ConstrainedConditionalGaussian();
ConstrainedConditionalGaussian(const std::string& key);
/**
* Used for unary factors that simply associate a name with a particular value
* Can use backsubstitution to solve trivially
* @param value is a fixed value for x in the form x = value
*/
ConstrainedConditionalGaussian(const Vector& value);
ConstrainedConditionalGaussian(const std::string& key, const Vector& value);
/**
* Used for unary factors of the form Ax=b
@ -48,7 +48,7 @@ public:
* @param b is the RHS of the equation
* @param A is the A matrix
*/
ConstrainedConditionalGaussian(const Vector& value, const Matrix& A);
ConstrainedConditionalGaussian(const std::string& key, const Vector& value, const Matrix& A);
/**
* Binary constructor of the form A1*x = b - A2*y
@ -58,7 +58,7 @@ public:
* @param parent is the string identifier for the parent node
* @param A2 is the A2 matrix
*/
ConstrainedConditionalGaussian(const Vector& b, const Matrix& A1,
ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1,
const std::string& parent, const Matrix& A2);
/**
@ -70,7 +70,7 @@ public:
* @param parentZ string id for z
* @param A3 is the A3 matrix
*/
ConstrainedConditionalGaussian(const Vector& b, const Matrix& A1,
ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1,
const std::string& parentY, const Matrix& A2,
const std::string& parentZ, const Matrix& A3);
@ -81,14 +81,9 @@ public:
* @param parents is the map of parents (Ai and xi from above)
* @param b is the rhs vector
*/
ConstrainedConditionalGaussian(const Matrix& A1,
ConstrainedConditionalGaussian(const std::string& key, const Matrix& A1,
const std::map<std::string, Matrix>& parents, const Vector& b);
/**
* Copy constructor
*/
ConstrainedConditionalGaussian(const ConstrainedConditionalGaussian& df);
virtual ~ConstrainedConditionalGaussian() {
}

5
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -6,6 +6,7 @@
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include "Ordering.h"
#include "ConstrainedLinearFactorGraph.h"
using namespace std;
@ -78,12 +79,12 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order
if (is_constrained(key))
{
ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key);
cbn->insert(key,ccg);
cbn->insert(ccg);
}
else
{
ConditionalGaussian::shared_ptr cg = eliminateOne<ConditionalGaussian>(key);
cbn->insert(key,cg);
cbn->insert(cg);
}
}

1
cpp/FactorGraph.h
View File

@ -11,6 +11,7 @@
#include <boost/shared_ptr.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/shared_ptr.hpp>

36
cpp/GaussianBayesNet.cpp
View File

@ -20,6 +20,7 @@ template class BayesNet<ConditionalGaussian>;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
/* ************************************************************************* */
boost::shared_ptr<VectorConfig> GaussianBayesNet::optimize() const
@ -27,11 +28,9 @@ boost::shared_ptr<VectorConfig> GaussianBayesNet::optimize() const
boost::shared_ptr<VectorConfig> result(new VectorConfig);
/** solve each node in turn in topological sort order (parents first)*/
BOOST_FOREACH(string key, keys_) {
const_iterator cg = nodes_.find(key); // get node
assert( cg != nodes_.end() ); // make sure it exists
Vector x = cg->second->solve(*result); // Solve for that variable
result->insert(key,x); // store result in partial solution
BOOST_REVERSE_FOREACH(ConditionalGaussian::shared_ptr cg,conditionals_) {
Vector x = cg->solve(*result); // Solve for that variable
result->insert(cg->key(),x); // store result in partial solution
}
return result;
}
@ -41,22 +40,19 @@ pair<Matrix,Vector> GaussianBayesNet::matrix() const {
// add the dimensions of all variables to get matrix dimension
// and at the same time create a mapping from keys to indices
size_t N=0; map<string,size_t> indices;
BOOST_REVERSE_FOREACH(string key, keys_) {
// find corresponding node
const_iterator it = nodes_.find(key);
indices.insert(make_pair(key,N));
N += it->second->dim();
size_t N=0; map<string,size_t> mapping;
BOOST_FOREACH(ConditionalGaussian::shared_ptr cg,conditionals_) {
mapping.insert(make_pair(cg->key(),N));
N += cg->dim();
}
// create matrix and copy in values
Matrix R = zeros(N,N);
Vector d(N);
string key; size_t I;
FOREACH_PAIR(key,I,indices) {
// find corresponding node
const_iterator it = nodes_.find(key);
ConditionalGaussian::shared_ptr cg = it->second;
string key; size_t I;
FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional
ConditionalGaussian::shared_ptr cg = (*this)[key];
// get RHS and copy to d
const Vector& d_ = cg->get_d();
@ -68,17 +64,17 @@ pair<Matrix,Vector> GaussianBayesNet::matrix() const {
const Matrix& R_ = cg->get_R();
for (size_t i=0;i<n;i++)
for(size_t j=0;j<n;j++)
R(I+i,I+j) = R_(i,j);
R(I+i,I+j) = R_(i,j);
// loop over S matrices and copy them into R
ConditionalGaussian::const_iterator keyS = cg->parentsBegin();
for (; keyS!=cg->parentsEnd(); keyS++) {
Matrix S = keyS->second; // get S matrix
const size_t m = S.size1(), n = S.size2(); // find S size
const size_t J = indices[keyS->first]; // find column index
const size_t J = mapping[keyS->first]; // find column index
for (size_t i=0;i<m;i++)
for(size_t j=0;j<n;j++)
R(I+i,J+j) = S(i,j);
for(size_t j=0;j<n;j++)
R(I+i,J+j) = S(i,j);
} // keyS
} // keyI

2
cpp/LinearConstraint.cpp
View File

@ -81,7 +81,7 @@ ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std
parents.erase(key);
// construct resulting CCG with parts
ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b));
ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(key, A1, parents, b));
return ccg;
}

20
cpp/LinearFactor.cpp
View File

@ -7,6 +7,10 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "Matrix.h"
#include "Ordering.h"
#include "ConditionalGaussian.h"
#include "LinearFactor.h"
using namespace std;
@ -20,6 +24,18 @@ using namespace gtsam;
typedef pair<const string, Matrix>& mypair;
/* ************************************************************************* */
LinearFactor::LinearFactor(const boost::shared_ptr<ConditionalGaussian> cg) :
b(cg->get_d()) {
As.insert(make_pair(cg->key(), cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
for (; it != cg->parentsEnd(); it++) {
const std::string& j = it->first;
const Matrix& Aj = it->second;
As.insert(make_pair(j, Aj));
}
}
/* ************************************************************************* */
LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
{
@ -186,7 +202,7 @@ LinearFactor::eliminate(const string& key)
// if this factor does not involve key, we exit with empty CG and LF
if (it==As.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian);
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf);
}
@ -227,7 +243,7 @@ LinearFactor::eliminate(const string& key)
} // column j
// create ConditionalGaussian with first n rows
ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(::sub(b,0,n), sub(R,0,n,0,n)) );
ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(key,::sub(b,0,n), sub(R,0,n,0,n)) );
// create linear factor with remaining rows
lf->set_b(::sub(b,n,m));

38
cpp/LinearFactor.h
View File

@ -9,23 +9,18 @@
#pragma once
#include <list>
#include <map>
#include <set>
#include <ostream>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <boost/serialization/map.hpp>
#include "Matrix.h"
#include "Factor.h"
#include "LinearFactorSet.h"
#include "ConditionalGaussian.h"
#include "Ordering.h"
#define CONSTRUCTOR
#include "Matrix.h"
#include "VectorConfig.h"
namespace gtsam {
class ConditionalGaussian;
class Ordering;
/**
* Base Class for a linear factor.
* LinearFactor is non-mutable (all methods const!).
@ -50,20 +45,17 @@ public:
}
/** Construct Null factor */
CONSTRUCTOR
LinearFactor(const Vector& b_in) :
b(b_in) { //TODO: add a way to initializing base class meaningfully
}
/** Construct unary factor */
CONSTRUCTOR
LinearFactor(const std::string& key1, const Matrix& A1, const Vector& b_in) :
b(b_in) {
As.insert(make_pair(key1, A1));
}
/** Construct binary factor */
CONSTRUCTOR
LinearFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const Vector& b_in) :
b(b_in) {
@ -72,7 +64,6 @@ public:
}
/** Construct ternary factor */
CONSTRUCTOR
LinearFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const std::string& key3,
const Matrix& A3, const Vector& b_in) :
@ -83,7 +74,6 @@ public:
}
/** Construct an n-ary factor */
CONSTRUCTOR
LinearFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
const Vector &b_in) :
b(b_in) {
@ -92,24 +82,12 @@ public:
}
/** Construct from Conditional Gaussian */
CONSTRUCTOR
LinearFactor(const std::string& key, const boost::shared_ptr<
ConditionalGaussian> cg) :
b(cg->get_d()) {
As.insert(make_pair(key, cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
for (; it != cg->parentsEnd(); it++) {
const std::string& j = it->first;
const Matrix& Aj = it->second;
As.insert(make_pair(j, Aj));
}
}
LinearFactor(const boost::shared_ptr<ConditionalGaussian> cg);
/**
* Constructor that combines a set of factors
* @param factors Set of factors to combine
*/
CONSTRUCTOR
LinearFactor(const std::vector<shared_ptr> & factors);
// Implementing Testable virtual functions
@ -212,7 +190,7 @@ public:
* @param key the key of the node to be eliminated
* @return a new factor and a conditional gaussian on the eliminated variable
*/
std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate(const std::string& key);
std::pair<boost::shared_ptr<ConditionalGaussian>, shared_ptr> eliminate(const std::string& key);
/**
* Take the factor f, and append to current matrices. Not very general.

5
cpp/LinearFactorGraph.cpp
View File

@ -15,6 +15,7 @@
#include "GaussianBayesNet.h"
#include "FactorGraph-inl.h"
#include "LinearFactorGraph.h"
#include "LinearFactorSet.h"
using namespace std;
using namespace gtsam;
@ -34,7 +35,7 @@ void LinearFactorGraph::setCBN(const GaussianBayesNet& CBN)
clear();
GaussianBayesNet::const_iterator it = CBN.begin();
for(; it != CBN.end(); it++) {
LinearFactor::shared_ptr lf(new LinearFactor(it->first, it->second));
LinearFactor::shared_ptr lf(new LinearFactor(*it));
push_back(lf);
}
}
@ -62,7 +63,7 @@ LinearFactorGraph::eliminate_partially(const Ordering& ordering)
BOOST_FOREACH(string key, ordering) {
ConditionalGaussian::shared_ptr cg = eliminateOne<ConditionalGaussian>(key);
chordalBayesNet->insert(key,cg);
chordalBayesNet->insert(cg);
}
return chordalBayesNet;

8
cpp/LinearFactorGraph.h
View File

@ -13,14 +13,14 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include "LinearFactor.h"
#include "VectorConfig.h"
#include "FactorGraph.h"
#include "GaussianBayesNet.h"
#include "LinearFactor.h"
namespace gtsam {
class Ordering;
class GaussianBayesNet;
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == LinearFactor

2
cpp/Makefile.am
View File

@ -161,7 +161,7 @@ testVSLAMFactor_LDADD = libgtsam.la
# The header files will be installed in ~/include/gtsam
headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h
headers = gtsam.h Value.h Testable.h Factor.h Conditional.h LinearFactorSet.h
headers += Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h
headers += $(sources:.cpp=.h)
# templates:

36
cpp/SymbolicConditional.h
View File

@ -12,14 +12,14 @@
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp> // TODO: make cpp file
#include "Testable.h"
#include "Conditional.h"
namespace gtsam {
/**
* Conditional node for use in a Bayes net
*/
class SymbolicConditional: Testable<SymbolicConditional> {
class SymbolicConditional: public Conditional {
private:
@ -33,20 +33,24 @@ namespace gtsam {
/**
* No parents
*/
SymbolicConditional() {
SymbolicConditional(const std::string& key) :
Conditional(key) {
}
/**
* Single parent
*/
SymbolicConditional(const std::string& parent) {
SymbolicConditional(const std::string& key, const std::string& parent) :
Conditional(key) {
parents_.push_back(parent);
}
/**
* Two parents
*/
SymbolicConditional(const std::string& parent1, const std::string& parent2) {
SymbolicConditional(const std::string& key, const std::string& parent1,
const std::string& parent2) :
Conditional(key) {
parents_.push_back(parent1);
parents_.push_back(parent2);
}
@ -54,24 +58,34 @@ namespace gtsam {
/**
* A list
*/
SymbolicConditional(const std::list<std::string>& parents):parents_(parents) {
SymbolicConditional(const std::string& key,
const std::list<std::string>& parents) :
Conditional(key), parents_(parents) {
}
/** print */
void print(const std::string& s = "SymbolicConditional") const {
std::cout << s;
std::cout << s << " P(" << key_ << " |";
BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
std::cout << std::endl;
std::cout << ")" << std::endl;
}
/** check equality */
bool equals(const SymbolicConditional& other, double tol = 1e-9) const {
return parents_ == other.parents_;
bool equals(const Conditional& c, double tol = 1e-9) const {
if (!Conditional::equals(c)) return false;
const SymbolicConditional* p = dynamic_cast<const SymbolicConditional*> (&c);
if (p == NULL) return false;
return parents_ == p->parents_;
}
/** return any parent */
/** return parents */
std::list<std::string> parents() const { return parents_;}
/** find the number of parents */
size_t nrParents() const {
return parents_.size();
}
};
} /// namespace gtsam

2
cpp/SymbolicFactor.cpp
View File

@ -59,7 +59,7 @@ namespace gtsam {
boost::shared_ptr<SymbolicFactor> lf(new SymbolicFactor(separator));
// create SymbolicConditional on separator
SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator));
SymbolicConditional::shared_ptr cg (new SymbolicConditional(key,separator));
return make_pair(cg,lf);
}

6
cpp/SymbolicFactorGraph.cpp
View File

@ -22,14 +22,14 @@ namespace gtsam {
SymbolicBayesNet::shared_ptr
SymbolicFactorGraph::eliminate(const Ordering& ordering)
{
SymbolicBayesNet::shared_ptr bayesChain (new SymbolicBayesNet());
SymbolicBayesNet::shared_ptr bayesNet (new SymbolicBayesNet());
BOOST_FOREACH(string key, ordering) {
SymbolicConditional::shared_ptr conditional = eliminateOne<SymbolicConditional>(key);
bayesChain->insert(key,conditional);
bayesNet->insert(conditional);
}
return bayesChain;
return bayesNet;
}
/* ************************************************************************* */

8
cpp/smallExample.cpp
View File

@ -200,11 +200,11 @@ GaussianBayesNet createSmallGaussianBayesNet()
// define nodes and specify in reverse topological sort (i.e. parents last)
ConditionalGaussian::shared_ptr
x(new ConditionalGaussian(d1,R11,"y",S12)),
y(new ConditionalGaussian(d2,R22));
Px_y(new ConditionalGaussian("x",d1,R11,"y",S12)),
Py(new ConditionalGaussian("y",d2,R22));
GaussianBayesNet cbn;
cbn.insert("x",x);
cbn.insert("y",y);
cbn.insert(Px_y);
cbn.insert(Py);
return cbn;
}

52
cpp/testBayesTree.cpp
View File

@ -4,31 +4,33 @@
* @author Frank Dellaert
*/
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "SymbolicBayesNet.h"
#include "GaussianBayesNet.h"
#include "Ordering.h"
#include "BayesTree-inl.h"
#include "SmallExample.h"
#include "smallExample.h"
using namespace gtsam;
// Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr B(new SymbolicConditional()), L(
new SymbolicConditional("B")), E(new SymbolicConditional("L", "B")), S(
new SymbolicConditional("L", "B")), T(new SymbolicConditional("E", "L")),
X(new SymbolicConditional("E"));
SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L(
new SymbolicConditional("L", "B")), E(
new SymbolicConditional("E", "L", "B")), S(new SymbolicConditional("S",
"L", "B")), T(new SymbolicConditional("T", "E", "L")), X(
new SymbolicConditional("X", "E"));
/* ************************************************************************* */
TEST( BayesTree, Front )
{
Front<SymbolicConditional> f1("B", B);
f1.add("L", L);
Front<SymbolicConditional> f2("L", L);
f2.add("B", B);
Front<SymbolicConditional> f1(B);
f1.add(L);
Front<SymbolicConditional> f2(L);
f2.add(B);
CHECK(f1.equals(f1));
CHECK(!f1.equals(f2));
}
@ -38,31 +40,31 @@ TEST( BayesTree, constructor )
{
// Create using insert
BayesTree<SymbolicConditional> bayesTree;
bayesTree.insert("B", B);
bayesTree.insert("L", L);
bayesTree.insert("E", E);
bayesTree.insert("S", S);
bayesTree.insert("T", T);
bayesTree.insert("X", X);
bayesTree.insert(B);
bayesTree.insert(L);
bayesTree.insert(E);
bayesTree.insert(S);
bayesTree.insert(T);
bayesTree.insert(X);
// Check Size
LONGS_EQUAL(4,bayesTree.size());
// Check root
Front<SymbolicConditional> expected_root("B", B);
expected_root.add("L", L);
expected_root.add("E", E);
Front<SymbolicConditional> expected_root(B);
expected_root.add(L);
expected_root.add(E);
Front<SymbolicConditional> actual_root = bayesTree.root();
CHECK(assert_equal(expected_root,actual_root));
// Create from symbolic Bayes chain in which we want to discover cliques
SymbolicBayesNet ASIA;
ASIA.insert("X", X);
ASIA.insert("T", T);
ASIA.insert("S", S);
ASIA.insert("E", E);
ASIA.insert("L", L);
ASIA.insert("B", B);
ASIA.insert(X);
ASIA.insert(T);
ASIA.insert(S);
ASIA.insert(E);
ASIA.insert(L);
ASIA.insert(B);
BayesTree<SymbolicConditional> bayesTree2(ASIA);
// Check whether the same

70
cpp/testConditionalGaussian.cpp
View File

@ -17,7 +17,6 @@
#include "Matrix.h"
#include "ConditionalGaussian.h"
using namespace gtsam;
/* ************************************************************************* */
@ -42,8 +41,8 @@ TEST( ConditionalGaussian, equals )
d(0) = 0.2; d(1) = 0.5;
ConditionalGaussian
expected(d, R, "x1", A1, "l1", A2),
actual(d, R, "x1", A1, "l1", A2);
expected("x",d, R, "x1", A1, "l1", A2),
actual("x",d, R, "x1", A1, "l1", A2);
CHECK( expected.equals(actual) );
@ -72,7 +71,7 @@ TEST( ConditionalGaussian, solve )
Vector d(2);
d(0) = 0.2; d(1) = 0.5;
ConditionalGaussian cg(d, R, "x1", A1, "l1", A2);
ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2);
Vector sx1(2);
sx1(0) = 0.2; sx1(1) = 0.5;
@ -96,39 +95,38 @@ TEST( ConditionalGaussian, solve )
#ifdef HAVE_BOOST_SERIALIZATION
TEST( ConditionalGaussian, serialize )
{
// // create a conditional gaussion node
// Matrix A1(2,2);
// A1(0,0) = 1 ; A1(1,0) = 2;
// A1(0,1) = 3 ; A1(1,1) = 4;
//
// Matrix A2(2,2);
// A2(0,0) = 6 ; A2(1,0) = 0.2;
// A2(0,1) = 8 ; A2(1,1) = 0.4;
//
// Matrix R(2,2);
// R(0,0) = 0.1 ; R(1,0) = 0.3;
// R(0,1) = 0.0 ; R(1,1) = 0.34;
//
// Vector d(2);
// d(0) = 0.2; d(1) = 0.5;
//
// ConditionalGaussian cg(d, R, "x1", A1, "l1", A2);
//
// //serialize the CG
// std::ostringstream in_archive_stream;
// boost::archive::text_oarchive in_archive(in_archive_stream);
// in_archive << cg;
// std::string serialized = in_archive_stream.str();
//
// //deserialize the CGg
// std::istringstream out_archive_stream(serialized);
// boost::archive::text_iarchive out_archive(out_archive_stream);
// ConditionalGaussian output;
// out_archive >> output;
//
// //check for equality
// CHECK(cg.equals(output));
// create a conditional gaussion node
Matrix A1(2,2);
A1(0,0) = 1 ; A1(1,0) = 2;
A1(0,1) = 3 ; A1(1,1) = 4;
Matrix A2(2,2);
A2(0,0) = 6 ; A2(1,0) = 0.2;
A2(0,1) = 8 ; A2(1,1) = 0.4;
Matrix R(2,2);
R(0,0) = 0.1 ; R(1,0) = 0.3;
R(0,1) = 0.0 ; R(1,1) = 0.34;
Vector d(2);
d(0) = 0.2; d(1) = 0.5;
ConditionalGaussian cg("x2", d, R, "x1", A1, "l1", A2);
//serialize the CG
std::ostringstream in_archive_stream;
boost::archive::text_oarchive in_archive(in_archive_stream);
in_archive << cg;
std::string serialized = in_archive_stream.str();
//deserialize the CGg
std::istringstream out_archive_stream(serialized);
boost::archive::text_iarchive out_archive(out_archive_stream);
ConditionalGaussian output;
out_archive >> output;
//check for equality
CHECK(cg.equals(output));
}
#endif //HAVE_BOOST_SERIALIZATION
/* ************************************************************************* */

10
cpp/testConstrainedConditionalGaussian.cpp
View File

@ -17,7 +17,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary1 )
// check unary constructor that doesn't require an R matrix
// assumed identity matrix
ConstrainedConditionalGaussian unary(v);
ConstrainedConditionalGaussian unary("x1",v);
VectorConfig fg;
fg.insert("x1", v);
@ -32,7 +32,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary2 )
// check unary constructor that makes use of a A matrix
Matrix A = eye(2) * 10;
ConstrainedConditionalGaussian unary(10*v, A);
ConstrainedConditionalGaussian unary("x1",10*v, A);
VectorConfig fg;
fg.insert("x1", v);
@ -51,7 +51,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary3 )
A(1,0) = 2.0 ; A(1,1) = 1.0;
Vector rhs = A*v;
ConstrainedConditionalGaussian unary(rhs, A);
ConstrainedConditionalGaussian unary("x1",rhs, A);
VectorConfig fg;
fg.insert("x1", v);
@ -84,7 +84,7 @@ TEST (ConstrainedConditionalGaussian, basic_binary1 )
Vector expected = Vector_(2, -3.3333, 0.6667);
ConstrainedConditionalGaussian binary(b, A1, "x1", A2);
ConstrainedConditionalGaussian binary("x2",b, A1, "x1", A2);
CHECK(assert_equal(expected, binary.solve(fg), 1e-4));
}
@ -119,7 +119,7 @@ TEST (ConstrainedConditionalGaussian, basic_ternary1 )
Vector expected = Vector_(2, 6.6667, -9.3333);
ConstrainedConditionalGaussian ternary(b, A1, "x1", A2, "x2", A3);
ConstrainedConditionalGaussian ternary("x3",b, A1, "x1", A2, "x2", A3);
CHECK(assert_equal(expected, ternary.solve(fg), 1e-4));
}

11
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -7,6 +7,7 @@
#include <CppUnitLite/TestHarness.h>
#include "ConstrainedLinearFactorGraph.h"
#include "LinearFactorGraph.h"
#include "Ordering.h"
#include "smallExample.h"
using namespace gtsam;
@ -40,7 +41,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
ConstrainedConditionalGaussian expectedCCG1(b2, Ax1, "y", Ay1);
ConstrainedConditionalGaussian expectedCCG1("x",b2, Ax1, "y", Ay1);
CHECK(expectedCCG1.equals(*((*cbn)["x"])));
// verify remaining factor on y
@ -64,7 +65,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
R(0, 0) = 74.5356; R(0, 1) = -59.6285;
R(1, 0) = 0.0; R(1, 1) = 44.7214;
Vector br = Vector_(2, 8.9443, 4.4721);
ConditionalGaussian expected2(br, R);
ConditionalGaussian expected2("y",br, R);
CHECK(expected2.equals(*((*cbn)["y"])));
}
@ -236,18 +237,18 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
// eliminate the constraint
ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x");
CHECK(cg1->size() == 1);
CHECK(cg1->nrParents() == 1);
CHECK(fg.nrFactors() == 1);
// eliminate the induced constraint
ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y");
CHECK(cg2->size() == 1);
CHECK(cg2->nrParents() == 1);
CHECK(fg.nrFactors() == 0);
// eliminate the linear factor
ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne<ConditionalGaussian>("z");
CHECK(cg3->nrParents() == 0);
CHECK(fg.size() == 0);
CHECK(cg3->size() == 0);
// solve piecewise
VectorConfig actual;

75
cpp/testGaussianBayesNet.cpp
View File

@ -20,6 +20,7 @@
#include "GaussianBayesNet.h"
#include "smallExample.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
@ -35,7 +36,7 @@ TEST( GaussianBayesNet, constructor )
d1(0) = 9; d2(0) = 5;
// define nodes and specify in reverse topological sort (i.e. parents last)
ConditionalGaussian x(d1,R11,"y",S12), y(d2,R22);
ConditionalGaussian x("x",d1,R11,"y",S12), y("y",d2,R22);
// check small example which uses constructor
GaussianBayesNet cbn = createSmallGaussianBayesNet();
@ -82,44 +83,40 @@ TEST( GaussianBayesNet, optimize )
#ifdef HAVE_BOOST_SERIALIZATION
TEST( GaussianBayesNet, serialize )
{
// //create a starting CBN
// GaussianBayesNet cbn = createSmallGaussianBayesNet();
//
// //serialize the CBN
// std::ostringstream in_archive_stream;
// boost::archive::text_oarchive in_archive(in_archive_stream);
// in_archive << cbn;
// std::string serialized = in_archive_stream.str();
//
// //DEBUG
// std::cout << "CBN Raw string: [" << serialized << "]" << std::endl;
//
// //remove newlines/carriage returns
// std::string clean;
// BOOST_FOREACH(char s, serialized)
// {
// if (s != '\n')
// {
// //copy in character
// clean.append(std::string(1,s));
// }
// else
// {
// std::cout << " Newline character found!" << std::endl;
// //replace with an identifiable string
// clean.append(std::string(1,' '));
// }
// }
//
//
// std::cout << "Cleaned CBN String: [" << clean << "]" << std::endl;
//
// //deserialize the CBN
// std::istringstream out_archive_stream(clean);
// boost::archive::text_iarchive out_archive(out_archive_stream);
// GaussianBayesNet output;
// out_archive >> output;
// CHECK(cbn.equals(output));
//create a starting CBN
GaussianBayesNet cbn = createSmallGaussianBayesNet();
//serialize the CBN
ostringstream in_archive_stream;
boost::archive::text_oarchive in_archive(in_archive_stream);
in_archive << cbn;
string serialized = in_archive_stream.str();
//DEBUG
cout << "CBN Raw string: [" << serialized << "]" << endl;
//remove newlines/carriage returns
string clean;
BOOST_FOREACH(char s, serialized) {
if (s != '\n') {
//copy in character
clean.append(string(1,s));
}
else {
cout << " Newline character found!" << endl;
//replace with an identifiable string
clean.append(string(1,' '));
}
}
cout << "Cleaned CBN String: [" << clean << "]" << endl;
//deserialize the CBN
istringstream out_archive_stream(clean);
boost::archive::text_iarchive out_archive(out_archive_stream);
GaussianBayesNet output;
out_archive >> output;
CHECK(cbn.equals(output));
}
#endif //HAVE_BOOST_SERIALIZATION

12
cpp/testLinearFactor.cpp
View File

@ -14,6 +14,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "Ordering.h"
#include "ConditionalGaussian.h"
#include "smallExample.h"
using namespace std;
@ -314,7 +316,7 @@ TEST( LinearFactor, eliminate )
+0.00,-8.94427
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian expectedCG(d,R11,"l1",S12,"x1",S13);
ConditionalGaussian expectedCG("x2",d,R11,"l1",S12,"x1",S13);
// the expected linear factor
Matrix Bl1 = Matrix_(2,2,
@ -384,7 +386,7 @@ TEST( LinearFactor, eliminate2 )
+0.00,-2.23607,+0.00,-8.94427
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian expectedCG(d,R11,"l1x1",S12);
ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12);
// the expected linear factor
Matrix Bl1x1 = Matrix_(2,4,
@ -424,7 +426,7 @@ TEST( LinearFactor, eliminate_empty )
boost::tie(actualCG,actualLF) = f.eliminate("x2");
// expected Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian expectedCG;
ConditionalGaussian expectedCG("x2");
// expected remaining factor is still empty :-)
LinearFactor expectedLF;
@ -495,8 +497,8 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
+0.00,-2.23607
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian(d,R11,"l1x1",S12) );
LinearFactor actualLF("x2",CG);
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12) );
LinearFactor actualLF(CG);
// actualLF.print();
LinearFactor expectedLF("x2",R11,"l1x1",S12,d);

21
cpp/testLinearFactorGraph.cpp
View File

@ -13,8 +13,11 @@ using namespace std;
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "Ordering.h"
#include "smallExample.h"
#include "GaussianBayesNet.h"
using namespace gtsam;
@ -182,7 +185,7 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
+0.00,-6.66667
);
Vector d(2); d(0) = -2; d(1) = -1.0/3.0;
ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13);
ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13);
CHECK(assert_equal(expected,*actual,tol));
}
@ -209,7 +212,7 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
+0.00,-8.94427
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian expected(d,R11,"l1",S12,"x1",S13);
ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13);
CHECK(assert_equal(expected,*actual,tol));
}
@ -235,7 +238,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
+0.00,-3.53553
);
Vector d(2); d(0) = -0.707107; d(1) = 1.76777;
ConditionalGaussian expected(d,R11,"x1",S12,"x2",S13);
ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13);
CHECK(assert_equal(expected,*actual,tol));
}
@ -248,7 +251,7 @@ TEST( LinearFactorGraph, eliminateAll )
0.0, 10};
Matrix R1 = Matrix_(2,2, data1);
Vector d1(2); d1(0) = -1; d1(1) = -1;
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian(d1, R1));
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1));
double data21[] = { 6.7082, 0.0,
0.0, 6.7082};
@ -257,7 +260,7 @@ TEST( LinearFactorGraph, eliminateAll )
0.0, -6.7082};
Matrix A1 = Matrix_(2,2, data22);
Vector d2(2); d2(0) = 0.0; d2(1) = 1.34164;
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian(d2, R2, "x1", A1));
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2, "x1", A1));
double data31[] = { 11.1803, 0.0,
0.0, 11.1803};
@ -270,12 +273,12 @@ TEST( LinearFactorGraph, eliminateAll )
Matrix A22 = Matrix_(2,2, data33);
Vector d3(2); d3(0) = 2.23607; d3(1) = -1.56525;
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22));
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3, "l1", A21, "x1", A22));
GaussianBayesNet expected;
expected.insert("x2", cg3);
expected.insert("l1", cg2);
expected.insert("x1", cg1);
expected.insert(cg3);
expected.insert(cg2);
expected.insert(cg1);
// Check one ordering
LinearFactorGraph fg1 = createLinearFactorGraph();

1
cpp/testNonlinearOptimizer.cpp
View File

@ -13,6 +13,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "Ordering.h"
#include "smallExample.h"
// template definitions

13
cpp/testSymbolicBayesNet.cpp
View File

@ -10,6 +10,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Ordering.h"
#include "smallExample.h"
#include "SymbolicBayesNet.h"
#include "SymbolicFactorGraph.h"
@ -22,13 +23,13 @@ TEST( SymbolicBayesNet, constructor )
{
// Create manually
SymbolicConditional::shared_ptr
x2(new SymbolicConditional("l1", "x1")),
l1(new SymbolicConditional("x1")),
x1(new SymbolicConditional());
x2(new SymbolicConditional("x2","l1", "x1")),
l1(new SymbolicConditional("l1","x1")),
x1(new SymbolicConditional("x1"));
SymbolicBayesNet expected;
expected.insert("x2",x2);
expected.insert("l1",l1);
expected.insert("x1",x1);
expected.insert(x2);
expected.insert(l1);
expected.insert(x1);
// Create from a factor graph
LinearFactorGraph factorGraph = createLinearFactorGraph();

15
cpp/testSymbolicFactorGraph.cpp
View File

@ -9,6 +9,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Ordering.h"
#include "smallExample.h"
#include "SymbolicFactorGraph.h"
#include "SymbolicBayesNet.h"
@ -114,7 +115,7 @@ TEST( LinearFactorGraph, eliminateOne )
fg.eliminateOne<SymbolicConditional>("x1");
// create expected symbolic Conditional
SymbolicConditional expected("l1","x2");
SymbolicConditional expected("x1","l1","x2");
CHECK(assert_equal(expected,*actual));
}
@ -123,14 +124,14 @@ TEST( LinearFactorGraph, eliminateOne )
TEST( LinearFactorGraph, eliminate )
{
// create expected Chordal bayes Net
SymbolicConditional::shared_ptr x2(new SymbolicConditional("l1", "x1"));
SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1"));
SymbolicConditional::shared_ptr x1(new SymbolicConditional());
SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1"));
SymbolicConditional::shared_ptr l1(new SymbolicConditional("l1", "x1"));
SymbolicConditional::shared_ptr x1(new SymbolicConditional("x1"));
SymbolicBayesNet expected;
expected.insert("x2", x2);
expected.insert("l1", l1);
expected.insert("x1", x1);
expected.insert(x2);
expected.insert(l1);
expected.insert(x1);
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();