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

Renamed ConditionalGaussian -> GaussianConditional

release/4.3a0
Alex Cunningham 2009-11-12 16:41:18 +00:00
parent 40f8ba740d
commit c7b86cec97
24 changed files with 166 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.cproject
View File

@ -353,9 +353,9 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildTarget>testConditionalGaussian.run</buildTarget> <buildTarget>testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>

2
cpp/BayesNet.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
* Bayes network * Bayes network
* This is the base class for SymbolicBayesNet, DiscreteBayesNet, and GaussianBayesNet * This is the base class for SymbolicBayesNet, DiscreteBayesNet, and GaussianBayesNet
* corresponding to what is used for the "Conditional" template argument: * corresponding to what is used for the "Conditional" template argument:
* a SymbolicConditional, ConditionalProbabilityTable, or a ConditionalGaussian * a SymbolicConditional, ConditionalProbabilityTable, or a GaussianConditional
*/ */
template<class Conditional> template<class Conditional>
class BayesNet: public Testable<BayesNet<Conditional> > { class BayesNet: public Testable<BayesNet<Conditional> > {

2
cpp/BayesTree.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
/** /**
* Bayes tree * Bayes tree
* Templated on the Conditional class, the type of node in the underlying Bayes chain. * Templated on the Conditional class, the type of node in the underlying Bayes chain.
* This could be a ConditionalProbabilityTable, a ConditionalGaussian, or a SymbolicConditional * This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional
*/ */
template<class Conditional> template<class Conditional>
class BayesTree: public Testable<BayesTree<Conditional> > { class BayesTree: public Testable<BayesTree<Conditional> > {

22
cpp/GaussianBayesNet.cpp
View File

@ -16,7 +16,7 @@ using namespace gtsam;
// Explicitly instantiate so we don't have to include everywhere // Explicitly instantiate so we don't have to include everywhere
#include "BayesNet-inl.h" #include "BayesNet-inl.h"
template class BayesNet<ConditionalGaussian>; template class BayesNet<GaussianConditional>;
// trick from some reading group // trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@ -27,8 +27,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) { GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) {
GaussianBayesNet bn; GaussianBayesNet bn;
ConditionalGaussian::shared_ptr GaussianConditional::shared_ptr
conditional(new ConditionalGaussian(key, Vector_(1,mu), eye(1), Vector_(1,sigma))); conditional(new GaussianConditional(key, Vector_(1,mu), eye(1), Vector_(1,sigma)));
bn.push_back(conditional); bn.push_back(conditional);
return bn; return bn;
} }
@ -37,8 +37,8 @@ GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) {
GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigma) { GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigma) {
GaussianBayesNet bn; GaussianBayesNet bn;
size_t n = mu.size(); size_t n = mu.size();
ConditionalGaussian::shared_ptr GaussianConditional::shared_ptr
conditional(new ConditionalGaussian(key, mu, eye(n), repeat(n,sigma))); conditional(new GaussianConditional(key, mu, eye(n), repeat(n,sigma)));
bn.push_back(conditional); bn.push_back(conditional);
return bn; return bn;
} }
@ -46,14 +46,14 @@ GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigm
/* ************************************************************************* */ /* ************************************************************************* */
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R,
const string& name1, Matrix S, Vector sigmas) { const string& name1, Matrix S, Vector sigmas) {
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, sigmas)); GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas));
bn.push_front(cg); bn.push_front(cg);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) { const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) {
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, name2, T, sigmas)); GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas));
bn.push_front(cg); bn.push_front(cg);
} }
@ -63,7 +63,7 @@ VectorConfig optimize(const GaussianBayesNet& bn)
VectorConfig result; VectorConfig result;
/** solve each node in turn in topological sort order (parents first)*/ /** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(ConditionalGaussian::shared_ptr cg, bn) { BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) {
Vector x = cg->solve(result); // Solve for that variable Vector x = cg->solve(result); // Solve for that variable
result.insert(cg->key(),x); // store result in partial solution result.insert(cg->key(),x); // store result in partial solution
} }
@ -76,7 +76,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// add the dimensions of all variables to get matrix dimension // add the dimensions of all variables to get matrix dimension
// and at the same time create a mapping from keys to indices // and at the same time create a mapping from keys to indices
size_t N=0; map<string,size_t> mapping; size_t N=0; map<string,size_t> mapping;
BOOST_FOREACH(ConditionalGaussian::shared_ptr cg,bn) { BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
mapping.insert(make_pair(cg->key(),N)); mapping.insert(make_pair(cg->key(),N));
N += cg->dim(); N += cg->dim();
} }
@ -87,7 +87,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
string key; size_t I; string key; size_t I;
FOREACH_PAIR(key,I,mapping) { FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional // find corresponding conditional
ConditionalGaussian::shared_ptr cg = bn[key]; GaussianConditional::shared_ptr cg = bn[key];
// get RHS and copy to d // get RHS and copy to d
const Vector& d_ = cg->get_d(); const Vector& d_ = cg->get_d();
@ -102,7 +102,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
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 // loop over S matrices and copy them into R
ConditionalGaussian::const_iterator keyS = cg->parentsBegin(); GaussianConditional::const_iterator keyS = cg->parentsBegin();
for (; keyS!=cg->parentsEnd(); keyS++) { for (; keyS!=cg->parentsEnd(); keyS++) {
Matrix S = keyS->second; // get S matrix Matrix S = keyS->second; // get S matrix
const size_t m = S.size1(), n = S.size2(); // find S size const size_t m = S.size1(), n = S.size2(); // find S size

4
cpp/GaussianBayesNet.h
View File

@ -11,13 +11,13 @@
#include <list> #include <list>
#include "ConditionalGaussian.h" #include "GaussianConditional.h"
#include "BayesNet.h" #include "BayesNet.h"
namespace gtsam { namespace gtsam {
/** A Bayes net made from linear-Gaussian densities */ /** A Bayes net made from linear-Gaussian densities */
typedef BayesNet<ConditionalGaussian> GaussianBayesNet; typedef BayesNet<GaussianConditional> GaussianBayesNet;
/** Create a scalar Gaussian */ /** Create a scalar Gaussian */
GaussianBayesNet scalarGaussian(const std::string& key, double mu=0.0, double sigma=1.0); GaussianBayesNet scalarGaussian(const std::string& key, double mu=0.0, double sigma=1.0);

22
cpp/ConditionalGaussian.cpp → cpp/GaussianConditional.cpp
View File

@ -1,5 +1,5 @@
/** /**
* @file ConditionalGaussian.cpp * @file GaussianConditional.cpp
* @brief Conditional Gaussian Base class * @brief Conditional Gaussian Base class
* @author Christian Potthast * @author Christian Potthast
*/ */
@ -7,26 +7,26 @@
#include <string.h> #include <string.h>
#include <boost/numeric/ublas/vector.hpp> #include <boost/numeric/ublas/vector.hpp>
#include "Ordering.h" #include "Ordering.h"
#include "ConditionalGaussian.h" #include "GaussianConditional.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key,Vector d, Matrix R, Vector sigmas) : GaussianConditional::GaussianConditional(const string& key,Vector d, Matrix R, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas),d_(d) Conditional (key), R_(R),sigmas_(sigmas),d_(d)
{ {
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, GaussianConditional::GaussianConditional(const string& key, Vector d, Matrix R,
const string& name1, Matrix S, Vector sigmas) : const string& name1, Matrix S, Vector sigmas) :
Conditional (key), R_(R), sigmas_(sigmas), d_(d) { Conditional (key), R_(R), sigmas_(sigmas), d_(d) {
parents_.insert(make_pair(name1, S)); parents_.insert(make_pair(name1, S));
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, GaussianConditional::GaussianConditional(const string& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) : const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas), d_(d) { Conditional (key), R_(R),sigmas_(sigmas), d_(d) {
parents_.insert(make_pair(name1, S)); parents_.insert(make_pair(name1, S));
@ -34,13 +34,13 @@ ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, GaussianConditional::GaussianConditional(const string& key,
const Vector& d, const Matrix& R, const map<string, Matrix>& parents, Vector sigmas) : const Vector& d, const Matrix& R, const map<string, Matrix>& parents, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas), d_(d), parents_(parents) { Conditional (key), R_(R),sigmas_(sigmas), d_(d), parents_(parents) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ConditionalGaussian::print(const string &s) const void GaussianConditional::print(const string &s) const
{ {
cout << s << ": density on " << key_ << endl; cout << s << ": density on " << key_ << endl;
gtsam::print(R_,"R"); gtsam::print(R_,"R");
@ -54,9 +54,9 @@ void ConditionalGaussian::print(const string &s) const
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool ConditionalGaussian::equals(const Conditional &c, double tol) const { bool GaussianConditional::equals(const Conditional &c, double tol) const {
if (!Conditional::equals(c)) return false; if (!Conditional::equals(c)) return false;
const ConditionalGaussian* p = dynamic_cast<const ConditionalGaussian*> (&c); const GaussianConditional* p = dynamic_cast<const GaussianConditional*> (&c);
if (p == NULL) return false; if (p == NULL) return false;
Parents::const_iterator it = parents_.begin(); Parents::const_iterator it = parents_.begin();
@ -85,7 +85,7 @@ bool ConditionalGaussian::equals(const Conditional &c, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<string> ConditionalGaussian::parents() const { list<string> GaussianConditional::parents() const {
list<string> result; list<string> result;
for (Parents::const_iterator it = parents_.begin(); it != parents_.end(); it++) for (Parents::const_iterator it = parents_.begin(); it != parents_.end(); it++)
result.push_back(it->first); result.push_back(it->first);
@ -93,7 +93,7 @@ list<string> ConditionalGaussian::parents() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector ConditionalGaussian::solve(const VectorConfig& x) const { Vector GaussianConditional::solve(const VectorConfig& x) const {
Vector rhs = d_; Vector rhs = d_;
for (Parents::const_iterator it = parents_.begin(); it for (Parents::const_iterator it = parents_.begin(); it
!= parents_.end(); it++) { != parents_.end(); it++) {

24
cpp/ConditionalGaussian.h → cpp/GaussianConditional.h
View File

@ -1,5 +1,5 @@
/** /**
* @file ConditionalGaussian.h * @file GaussianConditional.h
* @brief Conditional Gaussian Base class * @brief Conditional Gaussian Base class
* @author Christian Potthast * @author Christian Potthast
*/ */
@ -28,12 +28,12 @@ class Ordering;
* It has a set of parents y,z, etc. and implements a probability density on x. * 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 * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
*/ */
class ConditionalGaussian : public Conditional { class GaussianConditional : public Conditional {
public: public:
typedef std::map<std::string, Matrix> Parents; typedef std::map<std::string, Matrix> Parents;
typedef Parents::const_iterator const_iterator; typedef Parents::const_iterator const_iterator;
typedef boost::shared_ptr<ConditionalGaussian> shared_ptr; typedef boost::shared_ptr<GaussianConditional> shared_ptr;
protected: protected:
@ -52,41 +52,41 @@ protected:
public: public:
/** default constructor needed for serialization */ /** default constructor needed for serialization */
ConditionalGaussian(){} GaussianConditional(){}
/** constructor */ /** constructor */
ConditionalGaussian(const std::string& key) : GaussianConditional(const std::string& key) :
Conditional (key) {} Conditional (key) {}
/** constructor with no parents /** constructor with no parents
* |Rx-d| * |Rx-d|
*/ */
ConditionalGaussian(const std::string& key, Vector d, Matrix R, Vector sigmas); GaussianConditional(const std::string& key, Vector d, Matrix R, Vector sigmas);
/** constructor with only one parent /** constructor with only one parent
* |Rx+Sy-d| * |Rx+Sy-d|
*/ */
ConditionalGaussian(const std::string& key, Vector d, Matrix R, GaussianConditional(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, Vector sigmas); const std::string& name1, Matrix S, Vector sigmas);
/** constructor with two parents /** constructor with two parents
* |Rx+Sy+Tz-d| * |Rx+Sy+Tz-d|
*/ */
ConditionalGaussian(const std::string& key, Vector d, Matrix R, GaussianConditional(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas); const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas);
/** /**
* constructor with number of arbitrary parents * constructor with number of arbitrary parents
* |Rx+sum(Ai*xi)-d| * |Rx+sum(Ai*xi)-d|
*/ */
ConditionalGaussian(const std::string& key, const Vector& d, GaussianConditional(const std::string& key, const Vector& d,
const Matrix& R, const Parents& parents, Vector sigmas); const Matrix& R, const Parents& parents, Vector sigmas);
/** deconstructor */ /** deconstructor */
virtual ~ConditionalGaussian() {} virtual ~GaussianConditional() {}
/** print */ /** print */
void print(const std::string& = "ConditionalGaussian") const; void print(const std::string& = "GaussianConditional") const;
/** equals function */ /** equals function */
bool equals(const Conditional &cg, double tol = 1e-9) const; bool equals(const Conditional &cg, double tol = 1e-9) const;
@ -97,7 +97,7 @@ public:
/** return all parents */ /** return all parents */
std::list<std::string> parents() const; std::list<std::string> parents() const;
/** return stuff contained in ConditionalGaussian */ /** return stuff contained in GaussianConditional */
const Vector& get_d() const {return d_;} const Vector& get_d() const {return d_;}
const Matrix& get_R() const {return R_;} const Matrix& get_R() const {return R_;}
const Vector& get_sigmas() const {return sigmas_;} const Vector& get_sigmas() const {return sigmas_;}

12
cpp/GaussianFactor.cpp
View File

@ -11,7 +11,7 @@
#include "Matrix.h" #include "Matrix.h"
#include "Ordering.h" #include "Ordering.h"
#include "ConditionalGaussian.h" #include "GaussianConditional.h"
#include "GaussianFactor.h" #include "GaussianFactor.h"
using namespace std; using namespace std;
@ -26,7 +26,7 @@ using namespace gtsam;
typedef pair<const string, Matrix>& mypair; typedef pair<const string, Matrix>& mypair;
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(const boost::shared_ptr<ConditionalGaussian>& cg) : GaussianFactor::GaussianFactor(const boost::shared_ptr<GaussianConditional>& cg) :
b_(cg->get_d()) { b_(cg->get_d()) {
As_.insert(make_pair(cg->key(), cg->get_R())); As_.insert(make_pair(cg->key(), cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin(); std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
@ -282,11 +282,11 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, const size_t m,
/* Note, in place !!!! /* Note, in place !!!!
* Do incomplete QR factorization for the first n columns * Do incomplete QR factorization for the first n columns
* We will do QR on all matrices and on RHS * We will do QR on all matrices and on RHS
* Then take first n rows and make a ConditionalGaussian, * Then take first n rows and make a GaussianConditional,
* and last rows to make a new joint linear factor on separator * and last rows to make a new joint linear factor on separator
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
pair<ConditionalGaussian::shared_ptr, GaussianFactor::shared_ptr> pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const string& key) const GaussianFactor::eliminate(const string& key) const
{ {
bool verbose = false; bool verbose = false;
@ -297,7 +297,7 @@ GaussianFactor::eliminate(const string& key) const
if (it==As_.end()) { if (it==As_.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1 // Conditional Gaussian is just a parent-less node with P(x)=1
GaussianFactor::shared_ptr lf(new GaussianFactor); GaussianFactor::shared_ptr lf(new GaussianFactor);
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key)); GaussianConditional::shared_ptr cg(new GaussianConditional(key));
return make_pair(cg,lf); return make_pair(cg,lf);
} }
@ -339,7 +339,7 @@ GaussianFactor::eliminate(const string& key) const
} }
// create base conditional Gaussian // create base conditional Gaussian
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, GaussianConditional::shared_ptr cg(new GaussianConditional(key,
sub(d, 0, n1), // form d vector sub(d, 0, n1), // form d vector
sub(R, 0, n1, 0, n1), // form R matrix sub(R, 0, n1, 0, n1), // form R matrix
sub(newSigmas, 0, n1))); // get standard deviations sub(newSigmas, 0, n1))); // get standard deviations

6
cpp/GaussianFactor.h
View File

@ -19,7 +19,7 @@
namespace gtsam { namespace gtsam {
class ConditionalGaussian; class GaussianConditional;
class Ordering; class Ordering;
/** /**
@ -95,7 +95,7 @@ public:
} }
/** Construct from Conditional Gaussian */ /** Construct from Conditional Gaussian */
GaussianFactor(const boost::shared_ptr<ConditionalGaussian>& cg); GaussianFactor(const boost::shared_ptr<GaussianConditional>& cg);
/** /**
* Constructor that combines a set of factors * Constructor that combines a set of factors
@ -232,7 +232,7 @@ public:
* @param key the key of the node to be eliminated * @param key the key of the node to be eliminated
* @return a new factor and a conditional gaussian on the eliminated variable * @return a new factor and a conditional gaussian on the eliminated variable
*/ */
std::pair<boost::shared_ptr<ConditionalGaussian>, shared_ptr> std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
eliminate(const std::string& key) const; eliminate(const std::string& key) const;
/** /**

8
cpp/GaussianFactorGraph.cpp
View File

@ -42,9 +42,9 @@ set<string> GaussianFactorGraph::find_separator(const string& key) const
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConditionalGaussian::shared_ptr GaussianConditional::shared_ptr
GaussianFactorGraph::eliminateOne(const std::string& key) { GaussianFactorGraph::eliminateOne(const std::string& key) {
return gtsam::eliminateOne<GaussianFactor,ConditionalGaussian>(*this, key); return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*this, key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -53,7 +53,7 @@ GaussianFactorGraph::eliminate(const Ordering& ordering)
{ {
GaussianBayesNet chordalBayesNet; // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
ConditionalGaussian::shared_ptr cg = eliminateOne(key); GaussianConditional::shared_ptr cg = eliminateOne(key);
chordalBayesNet.push_back(cg); chordalBayesNet.push_back(cg);
} }
return chordalBayesNet; return chordalBayesNet;
@ -75,7 +75,7 @@ GaussianFactorGraph::eliminate_(const Ordering& ordering)
{ {
boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
ConditionalGaussian::shared_ptr cg = eliminateOne(key); GaussianConditional::shared_ptr cg = eliminateOne(key);
chordalBayesNet->push_back(cg); chordalBayesNet->push_back(cg);
} }
return chordalBayesNet; return chordalBayesNet;

2
cpp/GaussianFactorGraph.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
* Eliminates the factors from the factor graph through findAndRemoveFactors * Eliminates the factors from the factor graph through findAndRemoveFactors
* and adds a new factor on the separator to the factor graph * and adds a new factor on the separator to the factor graph
*/ */
ConditionalGaussian::shared_ptr eliminateOne(const std::string& key); GaussianConditional::shared_ptr eliminateOne(const std::string& key);
/** /**
* eliminate factor graph in place(!) in the given order, yielding * eliminate factor graph in place(!) in the given order, yielding

13
cpp/Makefile.am
View File

@ -85,16 +85,16 @@ testSymbolicBayesNet_LDADD = libgtsam.la
# Gaussian inference # Gaussian inference
headers += GaussianFactorSet.h headers += GaussianFactorSet.h
sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp ConditionalGaussian.cpp GaussianBayesNet.cpp sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testConditionalGaussian testGaussianBayesNet check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet
testVectorConfig_SOURCES = testVectorConfig.cpp testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_LDADD = libgtsam.la testVectorConfig_LDADD = libgtsam.la
testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp
testGaussianFactor_LDADD = libgtsam.la testGaussianFactor_LDADD = libgtsam.la
testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp
testGaussianFactorGraph_LDADD = libgtsam.la testGaussianFactorGraph_LDADD = libgtsam.la
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp
testConditionalGaussian_LDADD = libgtsam.la testGaussianConditional_LDADD = libgtsam.la
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp
testGaussianBayesNet_LDADD = libgtsam.la testGaussianBayesNet_LDADD = libgtsam.la
@ -112,13 +112,16 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
sources += NonlinearFactor.cpp sources += NonlinearFactor.cpp
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer sources += NonlinearEquality.cpp
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testNonlinearFactor_LDADD = libgtsam.la testNonlinearFactor_LDADD = libgtsam.la
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
testNonlinearFactorGraph_LDADD = libgtsam.la testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
testNonlinearOptimizer_LDADD = libgtsam.la testNonlinearOptimizer_LDADD = libgtsam.la
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la
# geometry # geometry
sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp

9
cpp/NonlinearEquality.cpp Normal file
View File

@ -0,0 +1,9 @@
/*
* @file NonlinearEquality.cpp
* @brief
* @author alexgc
*/
#include "NonlinearEquality.h"

26
cpp/NonlinearEquality.h Normal file
View File

@ -0,0 +1,26 @@
/*
* @file NonlinearEquality.h
* @brief Factor to handle enforced equality between factors
* @author Alex Cunningham
*/
#pragma once
#include "NonlinearFactor.h"
namespace gtsam {
/**
* An equality factor that forces either one variable to a constant,
* or a set of variables to be equal to each other.
* Throws an error at linearization if the constraints are not met.
*/
template<class Config>
class NonlinearEquality : public NonlinearFactor<Config>{
public:
NonlinearEquality();
virtual ~NonlinearEquality();
};
}

18
cpp/gtsam.h
View File

@ -52,19 +52,19 @@ class GaussianFactor {
pair<Matrix,Vector> matrix(const Ordering& ordering) const; pair<Matrix,Vector> matrix(const Ordering& ordering) const;
}; };
class ConditionalGaussian { class GaussianConditional {
ConditionalGaussian(); GaussianConditional();
ConditionalGaussian(string key, GaussianConditional(string key,
Vector d, Vector d,
Matrix R, Matrix R,
Vector sigmas); Vector sigmas);
ConditionalGaussian(string key, GaussianConditional(string key,
Vector d, Vector d,
Matrix R, Matrix R,
string name1, string name1,
Matrix S, Matrix S,
Vector sigmas); Vector sigmas);
ConditionalGaussian(string key, GaussianConditional(string key,
Vector d, Vector d,
Matrix R, Matrix R,
string name1, string name1,
@ -75,15 +75,15 @@ class ConditionalGaussian {
void print(string s) const; void print(string s) const;
Vector solve(const VectorConfig& x); Vector solve(const VectorConfig& x);
void add(string key, Matrix S); void add(string key, Matrix S);
bool equals(const ConditionalGaussian &cg, double tol) const; bool equals(const GaussianConditional &cg, double tol) const;
}; };
class GaussianBayesNet { class GaussianBayesNet {
GaussianBayesNet(); GaussianBayesNet();
void print(string s) const; void print(string s) const;
bool equals(const GaussianBayesNet& cbn, double tol) const; bool equals(const GaussianBayesNet& cbn, double tol) const;
void push_back(ConditionalGaussian* conditional); void push_back(GaussianConditional* conditional);
void push_front(ConditionalGaussian* conditional); void push_front(GaussianConditional* conditional);
}; };
class GaussianFactorGraph { class GaussianFactorGraph {
@ -96,7 +96,7 @@ class GaussianFactorGraph {
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactorGraph& lfgraph, double tol) const; bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
ConditionalGaussian* eliminateOne(string key); GaussianConditional* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering); GaussianBayesNet* eliminate_(const Ordering& ordering);
VectorConfig* optimize_(const Ordering& ordering); VectorConfig* optimize_(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const; pair<Matrix,Vector> matrix(const Ordering& ordering) const;

10
cpp/smallExample.cpp
View File

@ -204,9 +204,9 @@ GaussianBayesNet createSmallGaussianBayesNet()
Vector tau(1); tau(0) = 1.0; Vector tau(1); tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last) // define nodes and specify in reverse topological sort (i.e. parents last)
ConditionalGaussian::shared_ptr GaussianConditional::shared_ptr
Px_y(new ConditionalGaussian("x",d1,R11,"y",S12,tau)), Px_y(new GaussianConditional("x",d1,R11,"y",S12,tau)),
Py(new ConditionalGaussian("y",d2,R22,tau)); Py(new GaussianConditional("y",d2,R22,tau));
GaussianBayesNet cbn; GaussianBayesNet cbn;
cbn.push_back(Px_y); cbn.push_back(Px_y);
cbn.push_back(Py); cbn.push_back(Py);
@ -502,11 +502,11 @@ VectorConfig createMultiConstraintConfig() {
// Matrix R = eye(2); // Matrix R = eye(2);
// Vector d = c["x1"]; // Vector d = c["x1"];
// double sigma = 0.1; // double sigma = 0.1;
// ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); // GaussianConditional::shared_ptr f1(new GaussianConditional(d/sigma, R/sigma));
// cbn.insert("x1", f1); // cbn.insert("x1", f1);
// //
// // add a delta function to the cbn // // add a delta function to the cbn
// ConstrainedConditionalGaussian::shared_ptr f2(new ConstrainedConditionalGaussian); //(c["x0"], "x0")); // ConstrainedGaussianConditional::shared_ptr f2(new ConstrainedGaussianConditional); //(c["x0"], "x0"));
// cbn.insert_df("x0", f2); // cbn.insert_df("x0", f2);
// //
// return cbn; // return cbn;

6
cpp/testBayesTree.cpp
View File

@ -17,7 +17,7 @@ using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
typedef BayesTree<ConditionalGaussian> Gaussian; typedef BayesTree<GaussianConditional> Gaussian;
// Conditionals for ASIA example from the tutorial with A and D evidence // Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L( SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L(
@ -226,7 +226,7 @@ TEST( BayesTree, balanced_smoother_shortcuts )
CHECK(assert_equal(empty,actual2,1e-4)); CHECK(assert_equal(empty,actual2,1e-4));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4) // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
ConditionalGaussian::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
GaussianBayesNet expected3; expected3.push_back(p_x2_x4); GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
Gaussian::sharedClique C3 = bayesTree["x1"]; Gaussian::sharedClique C3 = bayesTree["x1"];
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R); GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
@ -252,7 +252,7 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma); push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R); FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
GaussianBayesNet actual = eliminate<GaussianFactor,ConditionalGaussian>(marginal,C3->keys()); GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys());
CHECK(assert_equal(expected,actual,1e-4)); CHECK(assert_equal(expected,actual,1e-4));
} }

2
cpp/testGaussianBayesNet.cpp
View File

@ -42,7 +42,7 @@ TEST( GaussianBayesNet, constructor )
sigmas(0) = 1.; sigmas(0) = 1.;
// define nodes and specify in reverse topological sort (i.e. parents last) // define nodes and specify in reverse topological sort (i.e. parents last)
ConditionalGaussian x("x",d1,R11,"y",S12, sigmas), y("y",d2,R22, sigmas); GaussianConditional x("x",d1,R11,"y",S12, sigmas), y("y",d2,R22, sigmas);
// check small example which uses constructor // check small example which uses constructor
GaussianBayesNet cbn = createSmallGaussianBayesNet(); GaussianBayesNet cbn = createSmallGaussianBayesNet();

18
cpp/testConditionalGaussian.cpp → cpp/testGaussianConditional.cpp
View File

@ -1,5 +1,5 @@
/** /**
* @file testConditionalGaussian.cpp * @file testGaussianConditional.cpp
* @brief Unit tests for Conditional gaussian * @brief Unit tests for Conditional gaussian
* @author Christian Potthast * @author Christian Potthast
**/ **/
@ -15,14 +15,14 @@
#endif //HAVE_BOOST_SERIALIZATION #endif //HAVE_BOOST_SERIALIZATION
#include "Matrix.h" #include "Matrix.h"
#include "ConditionalGaussian.h" #include "GaussianConditional.h"
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
/* unit test for equals */ /* unit test for equals */
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ConditionalGaussian, equals ) TEST( GaussianConditional, equals )
{ {
// create a conditional gaussian node // create a conditional gaussian node
Matrix A1(2,2); Matrix A1(2,2);
@ -44,7 +44,7 @@ TEST( ConditionalGaussian, equals )
Vector d(2); Vector d(2);
d(0) = 0.2; d(1) = 0.5; d(0) = 0.2; d(1) = 0.5;
ConditionalGaussian GaussianConditional
expected("x",d, R, "x1", A1, "l1", A2, tau), expected("x",d, R, "x1", A1, "l1", A2, tau),
actual("x",d, R, "x1", A1, "l1", A2, tau); actual("x",d, R, "x1", A1, "l1", A2, tau);
@ -55,7 +55,7 @@ TEST( ConditionalGaussian, equals )
/* ************************************************************************* */ /* ************************************************************************* */
/* unit test for solve */ /* unit test for solve */
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ConditionalGaussian, solve ) TEST( GaussianConditional, solve )
{ {
//expected solution //expected solution
Vector expected(2); Vector expected(2);
@ -76,7 +76,7 @@ TEST( ConditionalGaussian, solve )
Vector tau = ones(2); Vector tau = ones(2);
ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2, tau); GaussianConditional cg("x",d, R, "x1", A1, "l1", A2, tau);
Vector sx1(2); Vector sx1(2);
sx1(0) = 1.0; sx1(1) = 1.0; sx1(0) = 1.0; sx1(1) = 1.0;
@ -98,7 +98,7 @@ TEST( ConditionalGaussian, solve )
/* unit test for serialization */ /* unit test for serialization */
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef HAVE_BOOST_SERIALIZATION #ifdef HAVE_BOOST_SERIALIZATION
TEST( ConditionalGaussian, serialize ) TEST( GaussianConditional, serialize )
{ {
// create a conditional gaussion node // create a conditional gaussion node
Matrix A1(2,2); Matrix A1(2,2);
@ -116,7 +116,7 @@ TEST( ConditionalGaussian, serialize )
Vector d(2); Vector d(2);
d(0) = 0.2; d(1) = 0.5; d(0) = 0.2; d(1) = 0.5;
ConditionalGaussian cg("x2", d, R, "x1", A1, "l1", A2); GaussianConditional cg("x2", d, R, "x1", A1, "l1", A2);
//serialize the CG //serialize the CG
std::ostringstream in_archive_stream; std::ostringstream in_archive_stream;
@ -127,7 +127,7 @@ TEST( ConditionalGaussian, serialize )
//deserialize the CGg //deserialize the CGg
std::istringstream out_archive_stream(serialized); std::istringstream out_archive_stream(serialized);
boost::archive::text_iarchive out_archive(out_archive_stream); boost::archive::text_iarchive out_archive(out_archive_stream);
ConditionalGaussian output; GaussianConditional output;
out_archive >> output; out_archive >> output;
//check for equality //check for equality

28
cpp/testGaussianFactor.cpp
View File

@ -17,7 +17,7 @@ using namespace boost::assign;
#include "Matrix.h" #include "Matrix.h"
#include "Ordering.h" #include "Ordering.h"
#include "ConditionalGaussian.h" #include "GaussianConditional.h"
#include "smallExample.h" #include "smallExample.h"
using namespace std; using namespace std;
@ -314,7 +314,7 @@ TEST( GaussianFactor, eliminate )
GaussianFactor combined(lfg); GaussianFactor combined(lfg);
// eliminate the combined factor // eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG; GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2"); boost::tie(actualCG,actualLF) = combined.eliminate("x2");
@ -338,7 +338,7 @@ TEST( GaussianFactor, eliminate )
sigmas(1) = 1/sqrt(125.0); sigmas(1) = 1/sqrt(125.0);
// Check the conditional Gaussian // Check the conditional Gaussian
ConditionalGaussian expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas); GaussianConditional expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas);
// the expected linear factor // the expected linear factor
double sigma = 0.2236; double sigma = 0.2236;
@ -403,7 +403,7 @@ TEST( GaussianFactor, eliminate2 )
GaussianFactor combined(meas, b2, sigmas); GaussianFactor combined(meas, b2, sigmas);
// eliminate the combined factor // eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG; GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2"); boost::tie(actualCG,actualLF) = combined.eliminate("x2");
@ -422,7 +422,7 @@ TEST( GaussianFactor, eliminate2 )
x2Sigmas(0) = 0.0894427; x2Sigmas(0) = 0.0894427;
x2Sigmas(1) = 0.0894427; x2Sigmas(1) = 0.0894427;
ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas); GaussianConditional expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas);
// the expected linear factor // the expected linear factor
double sigma = 0.2236; double sigma = 0.2236;
@ -458,12 +458,12 @@ TEST( GaussianFactor, eliminate_empty )
GaussianFactor f; GaussianFactor f;
// eliminate the empty factor // eliminate the empty factor
ConditionalGaussian::shared_ptr actualCG; GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = f.eliminate("x2"); boost::tie(actualCG,actualLF) = f.eliminate("x2");
// expected Conditional Gaussian is just a parent-less node with P(x)=1 // expected Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian expectedCG("x2"); GaussianConditional expectedCG("x2");
// expected remaining factor is still empty :-) // expected remaining factor is still empty :-)
GaussianFactor expectedLF; GaussianFactor expectedLF;
@ -612,7 +612,7 @@ TEST( GaussianFactor, size )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactor, CONSTRUCTOR_ConditionalGaussian ) TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
{ {
Matrix R11 = Matrix_(2,2, Matrix R11 = Matrix_(2,2,
1.00, 0.00, 1.00, 0.00,
@ -628,7 +628,7 @@ TEST( GaussianFactor, CONSTRUCTOR_ConditionalGaussian )
sigmas(0) = 0.29907; sigmas(0) = 0.29907;
sigmas(1) = 0.29907; sigmas(1) = 0.29907;
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas)); GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l1x1",S12,sigmas));
GaussianFactor actualLF(CG); GaussianFactor actualLF(CG);
// actualLF.print(); // actualLF.print();
GaussianFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0)); GaussianFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0));
@ -645,7 +645,7 @@ TEST ( GaussianFactor, constraint_eliminate1 )
GaussianFactor lc(key, eye(2), v, 0.0); GaussianFactor lc(key, eye(2), v, 0.0);
// eliminate it // eliminate it
ConditionalGaussian::shared_ptr actualCG; GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = lc.eliminate("x0"); boost::tie(actualCG,actualLF) = lc.eliminate("x0");
@ -654,7 +654,7 @@ TEST ( GaussianFactor, constraint_eliminate1 )
// verify conditional Gaussian // verify conditional Gaussian
Vector sigmas = Vector_(2, 0.0, 0.0); Vector sigmas = Vector_(2, 0.0, 0.0);
ConditionalGaussian expCG("x0", v, eye(2), sigmas); GaussianConditional expCG("x0", v, eye(2), sigmas);
CHECK(assert_equal(expCG, *actualCG)); CHECK(assert_equal(expCG, *actualCG));
} }
@ -678,7 +678,7 @@ TEST ( GaussianFactor, constraint_eliminate2 )
GaussianFactor lc("x", A1, "y", A2, b, 0.0); GaussianFactor lc("x", A1, "y", A2, b, 0.0);
// eliminate x and verify results // eliminate x and verify results
ConditionalGaussian::shared_ptr actualCG; GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG, actualLF) = lc.eliminate("x"); boost::tie(actualCG, actualLF) = lc.eliminate("x");
@ -694,7 +694,7 @@ TEST ( GaussianFactor, constraint_eliminate2 )
1.0, 2.0, 1.0, 2.0,
0.0, 0.0); 0.0, 0.0);
Vector d = Vector_(2, 3.0, 0.6666); Vector d = Vector_(2, 3.0, 0.6666);
ConditionalGaussian expectedCG("x", d, R, "y", S, zero(2)); GaussianConditional expectedCG("x", d, R, "y", S, zero(2));
CHECK(assert_equal(expectedCG, *actualCG, 1e-4)); CHECK(assert_equal(expectedCG, *actualCG, 1e-4));
} }
@ -724,7 +724,7 @@ TEST ( GaussianFactor, constraint_eliminate3 )
// eliminate y from original graph // eliminate y from original graph
// NOTE: this will throw an exception, as // NOTE: this will throw an exception, as
// the leading matrix is rank deficient // the leading matrix is rank deficient
ConditionalGaussian::shared_ptr actualCG; GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
try { try {
boost::tie(actualCG, actualLF) = lc.eliminate("y"); boost::tie(actualCG, actualLF) = lc.eliminate("y");

14
cpp/testGaussianFactorGraph.cpp
View File

@ -190,12 +190,12 @@ TEST( GaussianFactorGraph, combine_factors_x2 )
TEST( GaussianFactorGraph, eliminateOne_x1 ) TEST( GaussianFactorGraph, eliminateOne_x1 )
{ {
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x1"); GaussianConditional::shared_ptr actual = fg.eliminateOne("x1");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15); Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15);
ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,sigma); GaussianConditional expected("x1",d,R11,"l1",S12,"x2",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -205,12 +205,12 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
TEST( GaussianFactorGraph, eliminateOne_x2 ) TEST( GaussianFactorGraph, eliminateOne_x2 )
{ {
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x2"); GaussianConditional::shared_ptr actual = fg.eliminateOne("x2");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I; Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427); Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427);
ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,sigma); GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -219,12 +219,12 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
TEST( GaussianFactorGraph, eliminateOne_l1 ) TEST( GaussianFactorGraph, eliminateOne_l1 )
{ {
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("l1"); GaussianConditional::shared_ptr actual = fg.eliminateOne("l1");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421); Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421);
ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,sigma); GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -353,7 +353,7 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
// Base FactorGraph only // Base FactorGraph only
FactorGraph<GaussianFactor> fg3(CBN); FactorGraph<GaussianFactor> fg3(CBN);
GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,ConditionalGaussian>(fg3,ord); GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,GaussianConditional>(fg3,ord);
CHECK(assert_equal(CBN,CBN3)); CHECK(assert_equal(CBN,CBN3));
} }

8
cpp/testInference.cpp
View File

@ -28,8 +28,8 @@ TEST(GaussianFactorGraph, createSmoother)
Ordering ordering; Ordering ordering;
GaussianBayesNet bayesNet = fg2.eliminate(ordering); GaussianBayesNet bayesNet = fg2.eliminate(ordering);
bayesNet.print("bayesNet"); bayesNet.print("bayesNet");
FactorGraph<GaussianFactor> p_x3 = marginalize<GaussianFactor,ConditionalGaussian>(bayesNet, Ordering("x3")); FactorGraph<GaussianFactor> p_x3 = marginalize<GaussianFactor,GaussianConditional>(bayesNet, Ordering("x3"));
FactorGraph<GaussianFactor> p_x1 = marginalize<GaussianFactor,ConditionalGaussian>(bayesNet, Ordering("x1")); FactorGraph<GaussianFactor> p_x1 = marginalize<GaussianFactor,GaussianConditional>(bayesNet, Ordering("x1"));
CHECK(assert_equal(p_x1,p_x3)); // should be the same because of symmetry CHECK(assert_equal(p_x1,p_x3)); // should be the same because of symmetry
} }
@ -39,10 +39,10 @@ TEST( Inference, marginals )
// create and marginalize a small Bayes net on "x" // create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet(); GaussianBayesNet cbn = createSmallGaussianBayesNet();
Ordering keys("x"); Ordering keys("x");
FactorGraph<GaussianFactor> fg = marginalize<GaussianFactor, ConditionalGaussian>(cbn,keys); FactorGraph<GaussianFactor> fg = marginalize<GaussianFactor, GaussianConditional>(cbn,keys);
// turn into Bayes net to test easily // turn into Bayes net to test easily
BayesNet<ConditionalGaussian> actual = eliminate<GaussianFactor,ConditionalGaussian>(fg,keys); BayesNet<GaussianConditional> actual = eliminate<GaussianFactor,GaussianConditional>(fg,keys);
// expected is just scalar Gaussian on x // expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2)); GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2));

16
cpp/testNonlinearEquality.cpp Normal file
View File

@ -0,0 +1,16 @@
/*
* @file testNonlinearEquality.cpp
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include "NonlinearEquality.h"
TEST ( NonlinearEquality, construction ) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
cpp/timeGaussianFactor.cpp
View File

@ -69,7 +69,7 @@ int main()
GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
long timeLog = clock(); long timeLog = clock();
int n = 1000000; int n = 1000000;
ConditionalGaussian::shared_ptr conditional; GaussianConditional::shared_ptr conditional;
GaussianFactor::shared_ptr factor; GaussianFactor::shared_ptr factor;
for(int i = 0; i < n; i++) for(int i = 0; i < n; i++)