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

BIG CHANGE: 

1) eliminate methods no longer return a shared pointer. Shared pointers are good for Factors and Conditionals (which are also non-copyable), because these are often passed around under the hood. However, a BayesNet is simple a list of shared pointers and hence does not cost a lot to return as an object (which is compiler-optimized anyway: there is no copy). So, the signature of all eliminate methods changed to simply return a BayesNet<> object (not a shared pointer).

2) GaussianBayesNet::optimize is now replaced by optimize(GaussianBayesNet) and returns a VectorConfig and not a shared pointer

3) GaussianBayesNet and SymbolicBayesNet are now simply typedefs, not derived classes. This is desirable because the BayesTree class uses templated methods that return BayesNet<Conditional>, not a specific BayesNet derived class.
release/4.3a0
Frank Dellaert 2009-11-09 07:04:26 +00:00
parent 7bd40e836d
commit a3de1964d7
24 changed files with 427 additions and 449 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cpp/BayesNet-inl.h
View File

@ -83,14 +83,14 @@ namespace gtsam {
BOOST_FOREACH(string key, keys) ord.push_back(key); BOOST_FOREACH(string key, keys) ord.push_back(key);
// eliminate to get joint // eliminate to get joint
typename BayesNet<Conditional>::shared_ptr joint = _eliminate<Factor,Conditional>(factorGraph,ord); BayesNet<Conditional> joint = _eliminate<Factor,Conditional>(factorGraph,ord);
// remove all integrands, P(K) = \int_I P(I|K) P(K) // remove all integrands, P(K) = \int_I P(I|K) P(K)
size_t nrIntegrands = ord.size()-keys.size(); size_t nrIntegrands = ord.size()-keys.size();
for(int i=0;i<nrIntegrands;i++) joint->pop_front(); for(int i=0;i<nrIntegrands;i++) joint.pop_front();
// joint is now only on keys, return it // joint is now only on keys, return it
return *joint; return joint;
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
cpp/BayesNet.h
View File

@ -30,8 +30,6 @@ namespace gtsam {
public: public:
typedef typename boost::shared_ptr<BayesNet<Conditional> >shared_ptr;
/** We store shared pointers to Conditional densities */ /** We store shared pointers to Conditional densities */
typedef typename boost::shared_ptr<Conditional> sharedConditional; typedef typename boost::shared_ptr<Conditional> sharedConditional;
typedef typename std::list<sharedConditional> Conditionals; typedef typename std::list<sharedConditional> Conditionals;

33
cpp/BayesTree-inl.h
View File

@ -61,19 +61,22 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional> template<class Conditional>
template<class Factor> template<class Factor>
typename BayesTree<Conditional>::sharedBayesNet BayesNet<Conditional>
BayesTree<Conditional>::Clique::shortcut(shared_ptr R) { BayesTree<Conditional>::Clique::shortcut(shared_ptr R) {
// A first base case is when this clique or its parent is the root, // A first base case is when this clique or its parent is the root,
// in which case we return an empty Bayes net. // in which case we return an empty Bayes net.
if (R.get()==this || parent_==R)
return sharedBayesNet(new BayesNet<Conditional>); if (R.get()==this || parent_==R) {
BayesNet<Conditional> empty;
return empty;
}
// The parent clique has a Conditional for each frontal node in Fp // The parent clique has a Conditional for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form // so we can obtain P(Fp|Sp) in factor graph form
FactorGraph<Factor> p_Fp_Sp(*parent_); FactorGraph<Factor> p_Fp_Sp(*parent_);
// If not the base case, obtain the parent shortcut P(Sp|R) as factors // If not the base case, obtain the parent shortcut P(Sp|R) as factors
FactorGraph<Factor> p_Sp_R(*parent_->shortcut<Factor>(R)); FactorGraph<Factor> p_Sp_R(parent_->shortcut<Factor>(R));
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
FactorGraph<Factor> p_Cp_R = combine(p_Fp_Sp, p_Sp_R); FactorGraph<Factor> p_Cp_R = combine(p_Fp_Sp, p_Sp_R);
@ -103,10 +106,10 @@ namespace gtsam {
BOOST_REVERSE_FOREACH(string key, integrands) ordering.push_front(key); BOOST_REVERSE_FOREACH(string key, integrands) ordering.push_front(key);
// eliminate to get marginal // eliminate to get marginal
sharedBayesNet p_S_R = _eliminate<Factor,Conditional>(p_Cp_R,ordering); BayesNet<Conditional> p_S_R = _eliminate<Factor,Conditional>(p_Cp_R,ordering);
// remove all integrands // remove all integrands
BOOST_FOREACH(string key, integrands) p_S_R->pop_front(); BOOST_FOREACH(string key, integrands) p_S_R.pop_front();
// return the parent shortcut P(Sp|R) // return the parent shortcut P(Sp|R)
return p_S_R; return p_S_R;
@ -126,12 +129,12 @@ namespace gtsam {
if (R.get()==this) return *R; if (R.get()==this) return *R;
// Combine P(F|S), P(S|R), and P(R) // Combine P(F|S), P(S|R), and P(R)
sharedBayesNet p_FSR = this->shortcut<Factor>(R); BayesNet<Conditional> p_FSR = this->shortcut<Factor>(R);
p_FSR->push_front(*this); p_FSR.push_front(*this);
p_FSR->push_back(*R); p_FSR.push_back(*R);
// Find marginal on the keys we are interested in // Find marginal on the keys we are interested in
return marginals<Factor>(*p_FSR,keys()); return marginals<Factor>(p_FSR,keys());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -145,11 +148,11 @@ namespace gtsam {
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
sharedBayesNet bn(new BayesNet<Conditional>); sharedBayesNet bn(new BayesNet<Conditional>);
if (!isRoot()) bn->push_back(*this); // P(F1|S1) if (!isRoot()) bn->push_back(*this); // P(F1|S1)
if (!isRoot()) bn->push_back(*(shortcut<Factor>(R))); // P(S1|R) if (!isRoot()) bn->push_back(shortcut<Factor>(R)); // P(S1|R)
if (!C2->isRoot()) bn->push_back(*C2); // P(F2|S2) if (!C2->isRoot()) bn->push_back(*C2); // P(F2|S2)
if (!C2->isRoot()) bn->push_back(*C2->shortcut<Factor>(R)); // P(S2|R) if (!C2->isRoot()) bn->push_back(C2->shortcut<Factor>(R)); // P(S2|R)
bn->push_back(*R); // P(R) bn->push_back(*R); // P(R)
// Find the keys of both C1 and C2 // Find the keys of both C1 and C2
Ordering keys12 = keys(); Ordering keys12 = keys();

3
cpp/BayesTree.h
View File

@ -64,8 +64,9 @@ namespace gtsam {
void printTree(const std::string& indent) const; void printTree(const std::string& indent) const;
/** return the conditional P(S|Root) on the separator given the root */ /** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version
template<class Factor> template<class Factor>
sharedBayesNet shortcut(shared_ptr root); BayesNet<Conditional> shortcut(shared_ptr root);
/** return the marginal P(C) of the clique */ /** return the marginal P(C) of the clique */
template<class Factor> template<class Factor>

14
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -70,8 +70,8 @@ bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double to
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering) { GaussianBayesNet ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering) {
GaussianBayesNet::shared_ptr cbn (new GaussianBayesNet()); GaussianBayesNet cbn;
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
// constraints take higher priority in elimination, so check if // constraints take higher priority in elimination, so check if
@ -79,12 +79,12 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order
if (is_constrained(key)) if (is_constrained(key))
{ {
ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key); ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key);
cbn->push_back(ccg); cbn.push_back(ccg);
} }
else else
{ {
ConditionalGaussian::shared_ptr cg = eliminateOne(key); ConditionalGaussian::shared_ptr cg = eliminateOne(key);
cbn->push_back(cg); cbn.push_back(cg);
} }
} }
@ -220,9 +220,9 @@ void ConstrainedLinearFactorGraph::update_constraints(const std::string& key,
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) { VectorConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) {
GaussianBayesNet::shared_ptr cbn = eliminate(ordering); GaussianBayesNet cbn = eliminate(ordering);
boost::shared_ptr<VectorConfig> newConfig = cbn->optimize(); VectorConfig newConfig = gtsam::optimize(cbn);
return *newConfig; return newConfig;
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
cpp/ConstrainedLinearFactorGraph.h
View File

@ -80,7 +80,7 @@ public:
* gaussian, with a different solving procedure. * gaussian, with a different solving procedure.
* @param ordering is the order to eliminate the variables * @param ordering is the order to eliminate the variables
*/ */
GaussianBayesNet::shared_ptr eliminate(const Ordering& ordering); GaussianBayesNet eliminate(const Ordering& ordering);
/** /**
* Picks one of the contraints in a set of constraints to eliminate * Picks one of the contraints in a set of constraints to eliminate

7
cpp/FactorGraph-inl.h
View File

@ -246,19 +246,20 @@ boost::shared_ptr<Conditional> _eliminateOne(FactorGraph<Factor>& graph, const s
// TODO: get rid of summy argument // TODO: get rid of summy argument
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor,class Conditional> template<class Factor,class Conditional>
boost::shared_ptr<BayesNet<Conditional> > BayesNet<Conditional>
_eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering) _eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering)
{ {
boost::shared_ptr<BayesNet<Conditional> > bayesNet (new BayesNet<Conditional>()); // empty BayesNet<Conditional> bayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
boost::shared_ptr<Conditional> cg = _eliminateOne<Factor,Conditional>(factorGraph,key); boost::shared_ptr<Conditional> cg = _eliminateOne<Factor,Conditional>(factorGraph,key);
bayesNet->push_back(cg); bayesNet.push_back(cg);
} }
return bayesNet; return bayesNet;
} }
/* ************************************************************************* */
template<class Factor> template<class Factor>
FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) { FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) {

2
cpp/FactorGraph.h
View File

@ -139,7 +139,7 @@ namespace gtsam {
* ordering, yielding a chordal Bayes net and (partially eliminated) FG * ordering, yielding a chordal Bayes net and (partially eliminated) FG
*/ */
template<class Factor, class Conditional> template<class Factor, class Conditional>
boost::shared_ptr<BayesNet<Conditional> > BayesNet<Conditional>
_eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering); _eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering);
/** /**

32
cpp/GaussianBayesNet.cpp
View File

@ -22,41 +22,47 @@ template class BayesNet<ConditionalGaussian>;
#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)
#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL) #define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet::GaussianBayesNet(const string& key, double mu, double sigma) { GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) {
GaussianBayesNet bn;
ConditionalGaussian::shared_ptr ConditionalGaussian::shared_ptr
conditional(new ConditionalGaussian(key, Vector_(1,mu), eye(1), Vector_(1,sigma))); conditional(new ConditionalGaussian(key, Vector_(1,mu), eye(1), Vector_(1,sigma)));
push_back(conditional); bn.push_back(conditional);
return bn;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet::GaussianBayesNet(const string& key, const Vector& mu, double sigma) { GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigma) {
GaussianBayesNet bn;
size_t n = mu.size(); size_t n = mu.size();
ConditionalGaussian::shared_ptr ConditionalGaussian::shared_ptr
conditional(new ConditionalGaussian(key, mu, eye(n), repeat(n,sigma))); conditional(new ConditionalGaussian(key, mu, eye(n), repeat(n,sigma)));
push_back(conditional); bn.push_back(conditional);
return bn;
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<VectorConfig> GaussianBayesNet::optimize() const VectorConfig optimize(const GaussianBayesNet& bn)
{ {
boost::shared_ptr<VectorConfig> result(new VectorConfig); 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,conditionals_) { BOOST_REVERSE_FOREACH(ConditionalGaussian::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
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> GaussianBayesNet::matrix() const { 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,conditionals_) { BOOST_FOREACH(ConditionalGaussian::shared_ptr cg,bn) {
mapping.insert(make_pair(cg->key(),N)); mapping.insert(make_pair(cg->key(),N));
N += cg->dim(); N += cg->dim();
} }
@ -67,7 +73,7 @@ pair<Matrix,Vector> GaussianBayesNet::matrix() const {
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 = (*this)[key]; ConditionalGaussian::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();
@ -98,3 +104,5 @@ pair<Matrix,Vector> GaussianBayesNet::matrix() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam

22
cpp/GaussianBayesNet.h
View File

@ -16,33 +16,23 @@
namespace gtsam { namespace gtsam {
/** Chordal Bayes Net, the result of eliminating a factor graph */ /** A Bayes net made from linear-Gaussian densities */
class GaussianBayesNet : public BayesNet<ConditionalGaussian> typedef BayesNet<ConditionalGaussian> GaussianBayesNet;
{
public:
typedef boost::shared_ptr<GaussianBayesNet> shared_ptr;
/** Construct an empty net */
GaussianBayesNet() {}
/** Create a scalar Gaussian */ /** Create a scalar Gaussian */
GaussianBayesNet(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);
/** Create a simple Gaussian on a single multivariate variable */ /** Create a simple Gaussian on a single multivariate variable */
GaussianBayesNet(const std::string& key, const Vector& mu, double sigma=1.0); GaussianBayesNet simpleGaussian(const std::string& key, const Vector& mu, double sigma=1.0);
/** Destructor */
virtual ~GaussianBayesNet() {}
/** /**
* optimize, i.e. return x = inv(R)*d * optimize, i.e. return x = inv(R)*d
*/ */
boost::shared_ptr<VectorConfig> optimize() const; VectorConfig optimize(const GaussianBayesNet&);
/** /**
* Return (dense) upper-triangular matrix representation * Return (dense) upper-triangular matrix representation
*/ */
std::pair<Matrix,Vector> matrix() const; std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);
};
} /// namespace gtsam } /// namespace gtsam

12
cpp/LinearFactorGraph.cpp
View File

@ -41,13 +41,13 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet::shared_ptr GaussianBayesNet
LinearFactorGraph::eliminate(const Ordering& ordering) LinearFactorGraph::eliminate(const Ordering& ordering)
{ {
GaussianBayesNet::shared_ptr chordalBayesNet (new GaussianBayesNet()); // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
ConditionalGaussian::shared_ptr cg = eliminateOne(key); ConditionalGaussian::shared_ptr cg = eliminateOne(key);
chordalBayesNet->push_back(cg); chordalBayesNet.push_back(cg);
} }
return chordalBayesNet; return chordalBayesNet;
} }
@ -56,12 +56,12 @@ LinearFactorGraph::eliminate(const Ordering& ordering)
VectorConfig LinearFactorGraph::optimize(const Ordering& ordering) VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
{ {
// eliminate all nodes in the given ordering -> chordal Bayes net // eliminate all nodes in the given ordering -> chordal Bayes net
GaussianBayesNet::shared_ptr chordalBayesNet = eliminate(ordering); GaussianBayesNet chordalBayesNet = eliminate(ordering);
// calculate new configuration (using backsubstitution) // calculate new configuration (using backsubstitution)
boost::shared_ptr<VectorConfig> newConfig = chordalBayesNet->optimize(); VectorConfig newConfig = ::optimize(chordalBayesNet);
return *newConfig; return newConfig;
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
cpp/LinearFactorGraph.h
View File

@ -75,7 +75,7 @@ namespace gtsam {
* a chordal Bayes net. Allows for passing an incomplete ordering * a chordal Bayes net. Allows for passing an incomplete ordering
* that does not completely eliminate the graph * that does not completely eliminate the graph
*/ */
boost::shared_ptr<GaussianBayesNet> eliminate(const Ordering& ordering); GaussianBayesNet eliminate(const Ordering& ordering);
/** /**
* optimize a linear factor graph * optimize a linear factor graph

6
cpp/SymbolicBayesNet.cpp
View File

@ -4,12 +4,6 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
#include "SymbolicBayesNet.h" #include "SymbolicBayesNet.h"
#include "BayesNet-inl.h" #include "BayesNet-inl.h"

47
cpp/SymbolicBayesNet.h
View File

@ -14,45 +14,30 @@
#include "Testable.h" #include "Testable.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "FactorGraph.h"
#include "SymbolicConditional.h" #include "SymbolicConditional.h"
namespace gtsam { namespace gtsam {
class Ordering;
/** /**
* Symbolic Bayes Chain, the (symbolic) result of eliminating a factor graph * Symbolic Bayes Chain, the (symbolic) result of eliminating a factor graph
*/ */
class SymbolicBayesNet: public BayesNet<SymbolicConditional> { typedef BayesNet<SymbolicConditional> SymbolicBayesNet;
public:
/** convenience typename for a shared pointer to this class */ /**
typedef boost::shared_ptr<SymbolicBayesNet> shared_ptr; * Construct from a Bayes net of any type
*/
/** template<class Conditional>
* Empty constructor SymbolicBayesNet symbolic(const BayesNet<Conditional>& bn) {
*/ SymbolicBayesNet result;
SymbolicBayesNet() {} typename BayesNet<Conditional>::const_iterator it = bn.begin();
for (; it != bn.end(); it++) {
/** boost::shared_ptr<Conditional> conditional = *it;
* Construct from a Bayes net of any type std::string key = conditional->key();
*/ std::list<std::string> parents = conditional->parents();
template<class Conditional> SymbolicConditional::shared_ptr c(new SymbolicConditional(key, parents));
SymbolicBayesNet(const BayesNet<Conditional>& bn) { result.push_back(c);
typename BayesNet<Conditional>::const_iterator it = bn.begin();
for(;it!=bn.end();it++) {
boost::shared_ptr<Conditional> conditional = *it;
std::string key = conditional->key();
std::list<std::string> parents = conditional->parents();
SymbolicConditional::shared_ptr c(new SymbolicConditional(key,parents));
push_back(c);
}
} }
return result;
/** Destructor */ }
virtual ~SymbolicBayesNet() {
}
};
} /// namespace gtsam } /// namespace gtsam

1
cpp/SymbolicConditional.h
View File

@ -10,6 +10,7 @@
#include <list> #include <list>
#include <string> #include <string>
#include <iostream>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp> // TODO: make cpp file #include <boost/foreach.hpp> // TODO: make cpp file
#include "Conditional.h" #include "Conditional.h"

8
cpp/SymbolicFactorGraph.cpp
View File

@ -19,20 +19,18 @@ namespace gtsam {
template class FactorGraph<SymbolicFactor>; template class FactorGraph<SymbolicFactor>;
/* ************************************************************************* */ /* ************************************************************************* */
SymbolicBayesNet::shared_ptr SymbolicBayesNet
SymbolicFactorGraph::eliminate(const Ordering& ordering) SymbolicFactorGraph::eliminate(const Ordering& ordering)
{ {
SymbolicBayesNet::shared_ptr bayesNet (new SymbolicBayesNet()); SymbolicBayesNet bayesNet;
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
SymbolicConditional::shared_ptr conditional = SymbolicConditional::shared_ptr conditional =
_eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key); _eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key);
bayesNet->push_back(conditional); bayesNet.push_back(conditional);
} }
return bayesNet; return bayesNet;
} }
/* ************************************************************************* */ /* ************************************************************************* */
} }

4
cpp/SymbolicFactorGraph.h
View File

@ -12,10 +12,10 @@
#include <list> #include <list>
#include "FactorGraph.h" #include "FactorGraph.h"
#include "SymbolicFactor.h" #include "SymbolicFactor.h"
#include "SymbolicBayesNet.h"
namespace gtsam { namespace gtsam {
class SymbolicBayesNet;
class SymbolicConditional; class SymbolicConditional;
/** Symbolic Factor Graph */ /** Symbolic Factor Graph */
@ -54,7 +54,7 @@ namespace gtsam {
* eliminate factor graph in place(!) in the given order, yielding * eliminate factor graph in place(!) in the given order, yielding
* a chordal Bayes net * a chordal Bayes net
*/ */
boost::shared_ptr<SymbolicBayesNet> eliminate(const Ordering& ordering); SymbolicBayesNet eliminate(const Ordering& ordering);
}; };

4
cpp/smallExample.cpp
View File

@ -218,7 +218,7 @@ GaussianBayesNet createSmallGaussianBayesNet()
/* ************************************************************************* */ /* ************************************************************************* */
// Some nonlinear functions to optimize // Some nonlinear functions to optimize
/* ************************************************************************* */ /* ************************************************************************* */
namespace optimize { namespace smallOptimize {
Vector h(const Vector& v) { Vector h(const Vector& v) {
double x = v(0); double x = v(0);
return Vector_(2,cos(x),sin(x)); return Vector_(2,cos(x),sin(x));
@ -236,7 +236,7 @@ boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactor
Vector z = Vector_(2,1.0,0.0); Vector z = Vector_(2,1.0,0.0);
double sigma = 0.1; double sigma = 0.1;
boost::shared_ptr<NonlinearFactor1> boost::shared_ptr<NonlinearFactor1>
factor(new NonlinearFactor1(z,sigma,&optimize::h,"x",&optimize::H)); factor(new NonlinearFactor1(z,sigma,&smallOptimize::h,"x",&smallOptimize::H));
fg->push_back(factor); fg->push_back(factor);
return fg; return fg;
} }

124
cpp/testBayesTree.cpp
View File

@ -101,40 +101,40 @@ TEST( BayesTree, linear_smoother_shortcuts )
ordering.push_back(symbol('x', t)); ordering.push_back(symbol('x', t));
// eliminate using the "natural" ordering // eliminate using the "natural" ordering
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
// Create the Bayes tree // Create the Bayes tree
Gaussian bayesTree(*chordalBayesNet); Gaussian bayesTree(chordalBayesNet);
LONGS_EQUAL(7,bayesTree.size()); LONGS_EQUAL(7,bayesTree.size());
// Check the conditional P(Root|Root) // Check the conditional P(Root|Root)
BayesNet<ConditionalGaussian> empty; GaussianBayesNet empty;
Gaussian::sharedClique R = bayesTree.root(); Gaussian::sharedClique R = bayesTree.root();
Gaussian::sharedBayesNet actual1 = R->shortcut<LinearFactor>(R); GaussianBayesNet actual1 = R->shortcut<LinearFactor>(R);
CHECK(assert_equal(empty,*actual1,1e-4)); CHECK(assert_equal(empty,actual1,1e-4));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
Gaussian::sharedClique C2 = bayesTree["x5"]; Gaussian::sharedClique C2 = bayesTree["x5"];
Gaussian::sharedBayesNet actual2 = C2->shortcut<LinearFactor>(R); GaussianBayesNet actual2 = C2->shortcut<LinearFactor>(R);
CHECK(assert_equal(empty,*actual2,1e-4)); CHECK(assert_equal(empty,actual2,1e-4));
// Check the conditional P(C3|Root) // Check the conditional P(C3|Root)
Vector sigma3 = repeat(2, 0.61808); Vector sigma3 = repeat(2, 0.61808);
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x5", zero(2), eye(2), "x6", A56, sigma3)); ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x5", zero(2), eye(2), "x6", A56, sigma3));
BayesNet<ConditionalGaussian> expected3; expected3.push_back(cg3); GaussianBayesNet expected3; expected3.push_back(cg3);
Gaussian::sharedClique C3 = bayesTree["x4"]; Gaussian::sharedClique C3 = bayesTree["x4"];
Gaussian::sharedBayesNet actual3 = C3->shortcut<LinearFactor>(R); GaussianBayesNet actual3 = C3->shortcut<LinearFactor>(R);
CHECK(assert_equal(expected3,*actual3,1e-4)); CHECK(assert_equal(expected3,actual3,1e-4));
// Check the conditional P(C4|Root) // Check the conditional P(C4|Root)
Vector sigma4 = repeat(2, 0.661968); Vector sigma4 = repeat(2, 0.661968);
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x6", A46, sigma4)); ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x6", A46, sigma4));
BayesNet<ConditionalGaussian> expected4; expected4.push_back(cg4); GaussianBayesNet expected4; expected4.push_back(cg4);
Gaussian::sharedClique C4 = bayesTree["x3"]; Gaussian::sharedClique C4 = bayesTree["x3"];
Gaussian::sharedBayesNet actual4 = C4->shortcut<LinearFactor>(R); GaussianBayesNet actual4 = C4->shortcut<LinearFactor>(R);
CHECK(assert_equal(expected4,*actual4,1e-4)); CHECK(assert_equal(expected4,actual4,1e-4));
} }
/* ************************************************************************* * /* ************************************************************************* *
@ -164,42 +164,42 @@ TEST( BayesTree, balanced_smoother_marginals )
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
// eliminate using a "nested dissection" ordering // eliminate using a "nested dissection" ordering
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
VectorConfig expectedSolution; VectorConfig expectedSolution;
BOOST_FOREACH(string key, ordering) BOOST_FOREACH(string key, ordering)
expectedSolution.insert(key,zero(2)); expectedSolution.insert(key,zero(2));
boost::shared_ptr<VectorConfig> actualSolution = chordalBayesNet->optimize(); VectorConfig actualSolution = optimize(chordalBayesNet);
CHECK(assert_equal(expectedSolution,*actualSolution,1e-4)); CHECK(assert_equal(expectedSolution,actualSolution,1e-4));
// Create the Bayes tree // Create the Bayes tree
Gaussian bayesTree(*chordalBayesNet); Gaussian bayesTree(chordalBayesNet);
LONGS_EQUAL(7,bayesTree.size()); LONGS_EQUAL(7,bayesTree.size());
// Check marginal on x1 // Check marginal on x1
GaussianBayesNet expected1("x1", zero(2), sigmax1); GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
BayesNet<ConditionalGaussian> actual1 = bayesTree.marginal<LinearFactor>("x1"); GaussianBayesNet actual1 = bayesTree.marginal<LinearFactor>("x1");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected1,actual1,1e-4)); CHECK(assert_equal(expected1,actual1,1e-4));
// Check marginal on x2 // Check marginal on x2
GaussianBayesNet expected2("x2", zero(2), sigmax2); GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2);
BayesNet<ConditionalGaussian> actual2 = bayesTree.marginal<LinearFactor>("x2"); GaussianBayesNet actual2 = bayesTree.marginal<LinearFactor>("x2");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected2,actual2,1e-4)); CHECK(assert_equal(expected2,actual2,1e-4));
// Check marginal on x3 // Check marginal on x3
GaussianBayesNet expected3("x3", zero(2), sigmax3); GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
BayesNet<ConditionalGaussian> actual3 = bayesTree.marginal<LinearFactor>("x3"); GaussianBayesNet actual3 = bayesTree.marginal<LinearFactor>("x3");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,1e-4));
// Check marginal on x4 // Check marginal on x4
GaussianBayesNet expected4("x4", zero(2), sigmax4); GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
BayesNet<ConditionalGaussian> actual4 = bayesTree.marginal<LinearFactor>("x4"); GaussianBayesNet actual4 = bayesTree.marginal<LinearFactor>("x4");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,1e-4));
// Check marginal on x7 (should be equal to x1) // Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7("x7", zero(2), sigmax7); GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
BayesNet<ConditionalGaussian> actual7 = bayesTree.marginal<LinearFactor>("x7"); GaussianBayesNet actual7 = bayesTree.marginal<LinearFactor>("x7");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected7,actual7,1e-4)); CHECK(assert_equal(expected7,actual7,1e-4));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -211,26 +211,26 @@ TEST( BayesTree, balanced_smoother_shortcuts )
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
// Create the Bayes tree // Create the Bayes tree
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
Gaussian bayesTree(*chordalBayesNet); Gaussian bayesTree(chordalBayesNet);
// Check the conditional P(Root|Root) // Check the conditional P(Root|Root)
BayesNet<ConditionalGaussian> empty; GaussianBayesNet empty;
Gaussian::sharedClique R = bayesTree.root(); Gaussian::sharedClique R = bayesTree.root();
Gaussian::sharedBayesNet actual1 = R->shortcut<LinearFactor>(R); GaussianBayesNet actual1 = R->shortcut<LinearFactor>(R);
CHECK(assert_equal(empty,*actual1,1e-4)); CHECK(assert_equal(empty,actual1,1e-4));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
Gaussian::sharedClique C2 = bayesTree["x3"]; Gaussian::sharedClique C2 = bayesTree["x3"];
Gaussian::sharedBayesNet actual2 = C2->shortcut<LinearFactor>(R); GaussianBayesNet actual2 = C2->shortcut<LinearFactor>(R);
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"]; ConditionalGaussian::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
BayesNet<ConditionalGaussian> 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"];
Gaussian::sharedBayesNet actual3 = C3->shortcut<LinearFactor>(R); GaussianBayesNet actual3 = C3->shortcut<LinearFactor>(R);
CHECK(assert_equal(expected3,*actual3,1e-4)); CHECK(assert_equal(expected3,actual3,1e-4));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -242,18 +242,18 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
// Create the Bayes tree // Create the Bayes tree
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
Gaussian bayesTree(*chordalBayesNet); Gaussian bayesTree(chordalBayesNet);
// Check the clique marginal P(C3) // Check the clique marginal P(C3)
GaussianBayesNet expected("x2",zero(2),sigmax2); GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2);
Vector sigma = repeat(2, 0.707107); Vector sigma = repeat(2, 0.707107);
Matrix A12 = (-0.5)*eye(2); Matrix A12 = (-0.5)*eye(2);
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x1", zero(2), eye(2), "x2", A12, sigma)); ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x1", zero(2), eye(2), "x2", A12, sigma));
expected.push_front(cg); expected.push_front(cg);
Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
BayesNet<ConditionalGaussian> actual = C3->marginal<LinearFactor>(R); GaussianBayesNet actual = C3->marginal<LinearFactor>(R);
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected,actual,1e-4)); CHECK(assert_equal(expected,actual,1e-4));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -265,44 +265,44 @@ TEST( BayesTree, balanced_smoother_joint )
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
// Create the Bayes tree // Create the Bayes tree
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
Gaussian bayesTree(*chordalBayesNet); Gaussian bayesTree(chordalBayesNet);
// Conditional density elements reused by both tests // Conditional density elements reused by both tests
Vector sigma = repeat(2, 0.786146); Vector sigma = repeat(2, 0.786146);
Matrix A = (-0.00429185)*eye(2); Matrix A = (-0.00429185)*eye(2);
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1("x7", zero(2), sigmax7); GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1", zero(2), eye(2), "x7", A, sigma)); ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1", zero(2), eye(2), "x7", A, sigma));
expected1.push_front(cg1); expected1.push_front(cg1);
BayesNet<ConditionalGaussian> actual1 = bayesTree.joint<LinearFactor>("x1","x7"); GaussianBayesNet actual1 = bayesTree.joint<LinearFactor>("x1","x7");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected1,actual1,1e-4)); CHECK(assert_equal(expected1,actual1,1e-4));
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
GaussianBayesNet expected2("x1", zero(2), sigmax1); GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("x7", zero(2), eye(2), "x1", A, sigma)); ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("x7", zero(2), eye(2), "x1", A, sigma));
expected2.push_front(cg2); expected2.push_front(cg2);
BayesNet<ConditionalGaussian> actual2 = bayesTree.joint<LinearFactor>("x7","x1"); GaussianBayesNet actual2 = bayesTree.joint<LinearFactor>("x7","x1");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected2,actual2,1e-4)); CHECK(assert_equal(expected2,actual2,1e-4));
// Check the joint density P(x1,x4), i.e. with a root variable // Check the joint density P(x1,x4), i.e. with a root variable
GaussianBayesNet expected3("x4", zero(2), sigmax4); GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4);
Vector sigma14 = repeat(2, 0.784465); Vector sigma14 = repeat(2, 0.784465);
Matrix A14 = (-0.0769231)*eye(2); Matrix A14 = (-0.0769231)*eye(2);
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x1", zero(2), eye(2), "x4", A14, sigma14)); ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x1", zero(2), eye(2), "x4", A14, sigma14));
expected3.push_front(cg3); expected3.push_front(cg3);
BayesNet<ConditionalGaussian> actual3 = bayesTree.joint<LinearFactor>("x1","x4"); GaussianBayesNet actual3 = bayesTree.joint<LinearFactor>("x1","x4");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,1e-4));
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
GaussianBayesNet expected4("x1", zero(2), sigmax1); GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1);
Vector sigma41 = repeat(2, 0.668096); Vector sigma41 = repeat(2, 0.668096);
Matrix A41 = (-0.055794)*eye(2); Matrix A41 = (-0.055794)*eye(2);
ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x1", A41, sigma41)); ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x1", A41, sigma41));
expected4.push_front(cg4); expected4.push_front(cg4);
BayesNet<ConditionalGaussian> actual4 = bayesTree.joint<LinearFactor>("x4","x1"); GaussianBayesNet actual4 = bayesTree.joint<LinearFactor>("x4","x1");
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,1e-4));
} }
/* ************************************************************************* */ /* ************************************************************************* */

504
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -26,12 +26,12 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
// eliminate x // eliminate x
Ordering ord; Ordering ord;
ord.push_back("x"); ord.push_back("x");
GaussianBayesNet::shared_ptr cbn = fg.eliminate(ord); GaussianBayesNet cbn = fg.eliminate(ord);
// verify result of elimination // verify result of elimination
// CBN of size 1, as we only eliminated X now // CBN of size 1, as we only eliminated X now
CHECK(fg.size() == 1); CHECK(fg.nrFactors() == 1);
CHECK(cbn->size() == 1); CHECK(cbn.size() == 1);
// We will have a "delta function" on X as a function of Y // We will have a "delta function" on X as a function of Y
// |1 2||x_1| = |1| - |10 0||y_1| // |1 2||x_1| = |1| - |10 0||y_1|
@ -42,258 +42,258 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
Matrix Ay1 = eye(2) * 10; Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0); Vector b2 = Vector_(2, 1.0, 2.0);
ConstrainedConditionalGaussian expectedCCG1("x",b2, Ax1, "y", Ay1); ConstrainedConditionalGaussian expectedCCG1("x",b2, Ax1, "y", Ay1);
CHECK(expectedCCG1.equals(*((*cbn)["x"]))); CHECK(expectedCCG1.equals(*(cbn["x"])));
// verify remaining factor on y // // verify remaining factor on y
// Gaussian factor on X becomes different Gaussian factor on Y // // Gaussian factor on X becomes different Gaussian factor on Y
Matrix Ap(2,2); // Matrix Ap(2,2);
Ap(0, 0) = 1.0; Ap(0, 1) = -2.0; // Ap(0, 0) = 1.0; Ap(0, 1) = -2.0;
Ap(1, 0) = -2.0; Ap(1, 1) = 1.0; // Ap(1, 0) = -2.0; Ap(1, 1) = 1.0;
Ap = 33.3333 * Ap; // Ap = 33.3333 * Ap;
Vector bp = Vector_(2, 0.0, -10.0); // Vector bp = Vector_(2, 0.0, -10.0);
double sigma1 = 1; // double sigma1 = 1;
LinearFactor expectedLF("y", Ap, bp,sigma1); // LinearFactor expectedLF("y", Ap, bp,sigma1);
CHECK(expectedLF.equals(*(fg[0]), 1e-4)); // CHECK(expectedLF.equals(*(fg[0]), 1e-4));
//
// eliminate y // // eliminate y
Ordering ord2; // Ordering ord2;
ord2.push_back("y"); // ord2.push_back("y");
cbn = fg.eliminate(ord2); // cbn = fg.eliminate(ord2);
//
// Check result // // Check result
CHECK(fg.size() == 0); // CHECK(fg.size() == 0);
Matrix R(2,2); // Matrix R(2,2);
R(0, 0) = 74.5356; R(0, 1) = -59.6285; // R(0, 0) = 74.5356; R(0, 1) = -59.6285;
R(1, 0) = 0.0; R(1, 1) = 44.7214; // R(1, 0) = 0.0; R(1, 1) = 44.7214;
Vector br = Vector_(2, 8.9443, 4.4721); // Vector br = Vector_(2, 8.9443, 4.4721);
Vector tau(2); // Vector tau(2);
tau(0) = R(0,0); // tau(0) = R(0,0);
tau(1) = R(1,1); // tau(1) = R(1,1);
//
// normalize the existing matrices // // normalize the existing matrices
Matrix N = eye(2,2); // Matrix N = eye(2,2);
N(0,0) = 1/tau(0); // N(0,0) = 1/tau(0);
N(1,1) = 1/tau(1); // N(1,1) = 1/tau(1);
R = N*R; // R = N*R;
ConditionalGaussian expected2("y",br, R, tau); // ConditionalGaussian expected2("y",br, R, tau);
CHECK(expected2.equals(*((*cbn)["y"]))); // CHECK(expected2.equals(*((*cbn)["y"])));
} }
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, optimize ) //TEST( ConstrainedLinearFactorGraph, optimize )
{ //{
// create graph // // create graph
ConstrainedLinearFactorGraph fg = createSingleConstraintGraph(); // ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
//
// perform optimization // // perform optimization
Ordering ord; // Ordering ord;
ord.push_back("y"); // ord.push_back("y");
ord.push_back("x"); // ord.push_back("x");
VectorConfig actual = fg.optimize(ord); // VectorConfig actual = fg.optimize(ord);
//
VectorConfig expected; // VectorConfig expected;
expected.insert("x", Vector_(2, 1.0, -1.0)); // expected.insert("x", Vector_(2, 1.0, -1.0));
expected.insert("y", Vector_(2, 0.2, 0.1)); // expected.insert("y", Vector_(2, 0.2, 0.1));
//
CHECK(expected.size() == actual.size()); // CHECK(expected.size() == actual.size());
CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); // CHECK(assert_equal(expected["x"], actual["x"], 1e-4));
CHECK(assert_equal(expected["y"], actual["y"], 1e-4)); // CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, optimize2 ) //TEST( ConstrainedLinearFactorGraph, optimize2 )
{ //{
// create graph // // create graph
ConstrainedLinearFactorGraph fg = createSingleConstraintGraph(); // ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
//
// perform optimization // // perform optimization
Ordering ord; // Ordering ord;
ord.push_back("x"); // ord.push_back("x");
ord.push_back("y"); // ord.push_back("y");
VectorConfig actual = fg.optimize(ord); // VectorConfig actual = fg.optimize(ord);
//
VectorConfig expected; // VectorConfig expected;
expected.insert("x", Vector_(2, 1.0, -1.0)); // expected.insert("x", Vector_(2, 1.0, -1.0));
expected.insert("y", Vector_(2, 0.2, 0.1)); // expected.insert("y", Vector_(2, 0.2, 0.1));
//
CHECK(expected.size() == actual.size()); // CHECK(expected.size() == actual.size());
CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); // Fails here: gets x = (-3, 1) // CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); // Fails here: gets x = (-3, 1)
CHECK(assert_equal(expected["y"], actual["y"], 1e-4)); // CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, is_constrained ) //TEST( ConstrainedLinearFactorGraph, is_constrained )
{ //{
// very simple check // // very simple check
ConstrainedLinearFactorGraph fg; // ConstrainedLinearFactorGraph fg;
CHECK(!fg.is_constrained("x")); // CHECK(!fg.is_constrained("x"));
//
// create simple graph // // create simple graph
Vector b = Vector_(2, 0.0, 0.0); // Vector b = Vector_(2, 0.0, 0.0);
LinearFactor::shared_ptr f1(new LinearFactor("x", eye(2), "y", eye(2), b,1)); // LinearFactor::shared_ptr f1(new LinearFactor("x", eye(2), "y", eye(2), b,1));
LinearFactor::shared_ptr f2(new LinearFactor("z", eye(2), "w", eye(2), b,1)); // LinearFactor::shared_ptr f2(new LinearFactor("z", eye(2), "w", eye(2), b,1));
LinearConstraint::shared_ptr f3(new LinearConstraint("y", eye(2), "z", eye(2), b)); // LinearConstraint::shared_ptr f3(new LinearConstraint("y", eye(2), "z", eye(2), b));
fg.push_back(f1); // fg.push_back(f1);
fg.push_back(f2); // fg.push_back(f2);
fg.push_back_constraint(f3); // fg.push_back_constraint(f3);
//
CHECK(fg.is_constrained("y")); // CHECK(fg.is_constrained("y"));
CHECK(fg.is_constrained("z")); // CHECK(fg.is_constrained("z"));
CHECK(!fg.is_constrained("x")); // CHECK(!fg.is_constrained("x"));
CHECK(!fg.is_constrained("w")); // CHECK(!fg.is_constrained("w"));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, get_constraint_separator ) //TEST( ConstrainedLinearFactorGraph, get_constraint_separator )
{ //{
ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph(); // ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
ConstrainedLinearFactorGraph fg2 = createMultiConstraintGraph(); // ConstrainedLinearFactorGraph fg2 = createMultiConstraintGraph();
LinearConstraint::shared_ptr lc1 = fg1.constraint_at(0); // LinearConstraint::shared_ptr lc1 = fg1.constraint_at(0);
LinearConstraint::shared_ptr lc2 = fg1.constraint_at(1); // LinearConstraint::shared_ptr lc2 = fg1.constraint_at(1);
//
vector<LinearConstraint::shared_ptr> actual1 = fg1.find_constraints_and_remove("y"); // vector<LinearConstraint::shared_ptr> actual1 = fg1.find_constraints_and_remove("y");
CHECK(fg1.size() == 2); // CHECK(fg1.size() == 2);
CHECK(actual1.size() == 1); // CHECK(actual1.size() == 1);
CHECK((*actual1.begin())->equals(*lc1)); // CHECK((*actual1.begin())->equals(*lc1));
//
vector<LinearConstraint::shared_ptr> actual2 = fg2.find_constraints_and_remove("x"); // vector<LinearConstraint::shared_ptr> actual2 = fg2.find_constraints_and_remove("x");
CHECK(fg2.size() == 1); // CHECK(fg2.size() == 1);
CHECK(actual2.size() == 2); // CHECK(actual2.size() == 2);
CHECK((*actual1.begin())->equals(*lc1)); // CHECK((*actual1.begin())->equals(*lc1));
LinearConstraint::shared_ptr act = *(++actual2.begin()); // LinearConstraint::shared_ptr act = *(++actual2.begin());
CHECK(act->equals(*lc2)); // CHECK(act->equals(*lc2));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, update_constraints ) //TEST( ConstrainedLinearFactorGraph, update_constraints )
{ //{
// create a graph // // create a graph
ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph(); // ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
//
// process constraints - picking first constraint on x // // process constraints - picking first constraint on x
vector<LinearConstraint::shared_ptr> constraints = fg1.find_constraints_and_remove("x"); // vector<LinearConstraint::shared_ptr> constraints = fg1.find_constraints_and_remove("x");
CHECK(constraints.size() == 2); // CHECK(constraints.size() == 2);
CHECK(fg1.size() == 1); // both constraints removed // CHECK(fg1.size() == 1); // both constraints removed
LinearConstraint::shared_ptr primary = constraints[0]; // LinearConstraint::shared_ptr primary = constraints[0];
LinearConstraint::shared_ptr updatee = constraints[1]; // LinearConstraint::shared_ptr updatee = constraints[1];
fg1.update_constraints("x", constraints, primary); // fg1.update_constraints("x", constraints, primary);
CHECK(fg1.size() == 2); // induced constraint added back // CHECK(fg1.size() == 2); // induced constraint added back
//
// expected induced constraint // // expected induced constraint
Matrix Ar(2,2); // Matrix Ar(2,2);
Ar(0, 0) = -16.6666; Ar(0, 1) = -6.6666; // Ar(0, 0) = -16.6666; Ar(0, 1) = -6.6666;
Ar(1, 0) = 10.0; Ar(1, 1) = 0.0; // Ar(1, 0) = 10.0; Ar(1, 1) = 0.0;
Matrix A22(2,2); // Matrix A22(2,2);
A22(0,0) = 1.0 ; A22(0,1) = 1.0; // A22(0,0) = 1.0 ; A22(0,1) = 1.0;
A22(1,0) = 1.0 ; A22(1,1) = 2.0; // A22(1,0) = 1.0 ; A22(1,1) = 2.0;
Vector br = Vector_(2, 0.0, 5.0); // Vector br = Vector_(2, 0.0, 5.0);
LinearConstraint::shared_ptr exp(new LinearConstraint("y", Ar, "z", A22, br)); // LinearConstraint::shared_ptr exp(new LinearConstraint("y", Ar, "z", A22, br));
//
// evaluate // // evaluate
CHECK(assert_equal(*(fg1.constraint_at(0)), *exp, 1e-4)); // CHECK(assert_equal(*(fg1.constraint_at(0)), *exp, 1e-4));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, find_constraints_and_remove ) //TEST( ConstrainedLinearFactorGraph, find_constraints_and_remove )
{ //{
// constraint 1 // // constraint 1
Matrix A11(2,2); // Matrix A11(2,2);
A11(0,0) = 1.0 ; A11(0,1) = 2.0; // A11(0,0) = 1.0 ; A11(0,1) = 2.0;
A11(1,0) = 2.0 ; A11(1,1) = 1.0; // A11(1,0) = 2.0 ; A11(1,1) = 1.0;
//
Matrix A12(2,2); // Matrix A12(2,2);
A12(0,0) = 10.0 ; A12(0,1) = 0.0; // A12(0,0) = 10.0 ; A12(0,1) = 0.0;
A12(1,0) = 0.0 ; A12(1,1) = 10.0; // A12(1,0) = 0.0 ; A12(1,1) = 10.0;
//
Vector b1(2); // Vector b1(2);
b1(0) = 1.0; b1(1) = 2.0; // b1(0) = 1.0; b1(1) = 2.0;
LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1)); // LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
//
// constraint 2 // // constraint 2
Matrix A21(2,2); // Matrix A21(2,2);
A21(0,0) = 3.0 ; A21(0,1) = 4.0; // A21(0,0) = 3.0 ; A21(0,1) = 4.0;
A21(1,0) = -1.0 ; A21(1,1) = -2.0; // A21(1,0) = -1.0 ; A21(1,1) = -2.0;
//
Matrix A22(2,2); // Matrix A22(2,2);
A22(0,0) = 1.0 ; A22(0,1) = 1.0; // A22(0,0) = 1.0 ; A22(0,1) = 1.0;
A22(1,0) = 1.0 ; A22(1,1) = 2.0; // A22(1,0) = 1.0 ; A22(1,1) = 2.0;
//
Vector b2(2); // Vector b2(2);
b2(0) = 3.0; b2(1) = 4.0; // b2(0) = 3.0; b2(1) = 4.0;
LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2)); // LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
//
// construct the graph // // construct the graph
ConstrainedLinearFactorGraph fg1; // ConstrainedLinearFactorGraph fg1;
fg1.push_back_constraint(lc1); // fg1.push_back_constraint(lc1);
fg1.push_back_constraint(lc2); // fg1.push_back_constraint(lc2);
//
// constraints on x // // constraints on x
vector<LinearConstraint::shared_ptr> expected1, actual1; // vector<LinearConstraint::shared_ptr> expected1, actual1;
expected1.push_back(lc1); // expected1.push_back(lc1);
expected1.push_back(lc2); // expected1.push_back(lc2);
actual1 = fg1.find_constraints_and_remove("x"); // actual1 = fg1.find_constraints_and_remove("x");
CHECK(fg1.size() == 0); // CHECK(fg1.size() == 0);
CHECK(expected1.size() == actual1.size()); // CHECK(expected1.size() == actual1.size());
vector<LinearConstraint::shared_ptr>::const_iterator exp1, act1; // vector<LinearConstraint::shared_ptr>::const_iterator exp1, act1;
for(exp1=expected1.begin(), act1=actual1.begin(); act1 != actual1.end(); ++act1, ++exp1) { // for(exp1=expected1.begin(), act1=actual1.begin(); act1 != actual1.end(); ++act1, ++exp1) {
CHECK((*exp1)->equals(**act1)); // CHECK((*exp1)->equals(**act1));
} // }
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint ) //TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
{ //{
ConstrainedLinearFactorGraph fg = createMultiConstraintGraph(); // ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
//
// eliminate the constraint // // eliminate the constraint
ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x"); // ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x");
CHECK(cg1->nrParents() == 1); // CHECK(cg1->nrParents() == 1);
CHECK(fg.nrFactors() == 1); // CHECK(fg.nrFactors() == 1);
//
// eliminate the induced constraint // // eliminate the induced constraint
ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y"); // ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y");
CHECK(cg2->nrParents() == 1); // CHECK(cg2->nrParents() == 1);
CHECK(fg.nrFactors() == 0); // CHECK(fg.nrFactors() == 0);
//
// eliminate the linear factor // // eliminate the linear factor
ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne("z"); // ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne("z");
CHECK(cg3->nrParents() == 0); // CHECK(cg3->nrParents() == 0);
CHECK(fg.size() == 0); // CHECK(fg.size() == 0);
//
// solve piecewise // // solve piecewise
VectorConfig actual; // VectorConfig actual;
Vector act_z = cg3->solve(actual); // Vector act_z = cg3->solve(actual);
actual.insert("z", act_z); // actual.insert("z", act_z);
CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4)); // CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4));
Vector act_y = cg2->solve(actual); // Vector act_y = cg2->solve(actual);
actual.insert("y", act_y); // actual.insert("y", act_y);
CHECK(assert_equal(act_y, Vector_(2, -0.1, 0.4), 1e-4)); // CHECK(assert_equal(act_y, Vector_(2, -0.1, 0.4), 1e-4));
Vector act_x = cg1->solve(actual); // Vector act_x = cg1->solve(actual);
CHECK(assert_equal(act_x, Vector_(2, -2.0, 2.0), 1e-4)); // CHECK(assert_equal(act_x, Vector_(2, -2.0, 2.0), 1e-4));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) //TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
{ //{
ConstrainedLinearFactorGraph fg = createMultiConstraintGraph(); // ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
// solve the graph // // solve the graph
Ordering ord; // Ordering ord;
ord.push_back("x"); // ord.push_back("x");
ord.push_back("y"); // ord.push_back("y");
ord.push_back("z"); // ord.push_back("z");
//
VectorConfig actual = fg.optimize(ord); // VectorConfig actual = fg.optimize(ord);
//
// verify // // verify
VectorConfig expected; // VectorConfig expected;
expected.insert("x", Vector_(2, -2.0, 2.0)); // expected.insert("x", Vector_(2, -2.0, 2.0));
expected.insert("y", Vector_(2, -0.1, 0.4)); // expected.insert("y", Vector_(2, -0.1, 0.4));
expected.insert("z", Vector_(2, -4.0, 5.0)); // expected.insert("z", Vector_(2, -4.0, 5.0));
CHECK(expected.size() == actual.size()); // CHECK(expected.size() == actual.size());
CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); // CHECK(assert_equal(expected["x"], actual["x"], 1e-4));
CHECK(assert_equal(expected["y"], actual["y"], 1e-4)); // CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
CHECK(assert_equal(expected["z"], actual["z"], 1e-4)); // CHECK(assert_equal(expected["z"], actual["z"], 1e-4));
} //}
/* ************************************************************************* */ /* ************************************************************************* */
// OLD TESTS - should be ported into the new structure when possible // OLD TESTS - should be ported into the new structure when possible

12
cpp/testGaussianBayesNet.cpp
View File

@ -57,7 +57,7 @@ TEST( GaussianBayesNet, matrix )
GaussianBayesNet cbn = createSmallGaussianBayesNet(); GaussianBayesNet cbn = createSmallGaussianBayesNet();
Matrix R; Vector d; Matrix R; Vector d;
boost::tie(R,d) = cbn.matrix(); // find matrix and RHS boost::tie(R,d) = matrix(cbn); // find matrix and RHS
Matrix R1 = Matrix_(2,2, Matrix R1 = Matrix_(2,2,
1.0, 1.0, 1.0, 1.0,
@ -73,7 +73,7 @@ TEST( GaussianBayesNet, matrix )
TEST( GaussianBayesNet, optimize ) TEST( GaussianBayesNet, optimize )
{ {
GaussianBayesNet cbn = createSmallGaussianBayesNet(); GaussianBayesNet cbn = createSmallGaussianBayesNet();
boost::shared_ptr<VectorConfig> actual = cbn.optimize(); VectorConfig actual = optimize(cbn);
VectorConfig expected; VectorConfig expected;
Vector x(1), y(1); Vector x(1), y(1);
@ -81,7 +81,7 @@ TEST( GaussianBayesNet, optimize )
expected.insert("x",x); expected.insert("x",x);
expected.insert("y",y); expected.insert("y",y);
CHECK(actual->equals(expected)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -90,11 +90,11 @@ TEST( GaussianBayesNet, 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");
BayesNet<ConditionalGaussian> actual = marginals<LinearFactor>(cbn,keys); GaussianBayesNet actual = marginals<LinearFactor>(cbn,keys);
// expected is just scalar Gaussian on x // expected is just scalar Gaussian on x
GaussianBayesNet expected("x",4,sqrt(2)); GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2));
CHECK(assert_equal((BayesNet<ConditionalGaussian>)expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */

21
cpp/testLinearFactorGraph.cpp
View File

@ -315,8 +315,8 @@ TEST( LinearFactorGraph, eliminateAll )
LinearFactorGraph fg1 = createLinearFactorGraph(); LinearFactorGraph fg1 = createLinearFactorGraph();
Ordering ord1; Ordering ord1;
ord1 += "x2","l1","x1"; ord1 += "x2","l1","x1";
GaussianBayesNet::shared_ptr actual = fg1.eliminate(ord1); GaussianBayesNet actual = fg1.eliminate(ord1);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,actual,tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -346,7 +346,7 @@ TEST( LinearFactorGraph, copying )
// now eliminate the copy // now eliminate the copy
Ordering ord1; Ordering ord1;
ord1 += "x2","l1","x1"; ord1 += "x2","l1","x1";
GaussianBayesNet::shared_ptr actual1 = copy.eliminate(ord1); GaussianBayesNet actual1 = copy.eliminate(ord1);
// Create the same graph, but not by copying // Create the same graph, but not by copying
LinearFactorGraph expected = createLinearFactorGraph(); LinearFactorGraph expected = createLinearFactorGraph();
@ -411,18 +411,17 @@ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet )
// render with a given ordering // render with a given ordering
Ordering ord; Ordering ord;
ord += "x2","l1","x1"; ord += "x2","l1","x1";
GaussianBayesNet::shared_ptr CBN = fg.eliminate(ord); GaussianBayesNet CBN = fg.eliminate(ord);
// True LinearFactorGraph // True LinearFactorGraph
LinearFactorGraph fg2(*CBN); LinearFactorGraph fg2(CBN);
GaussianBayesNet::shared_ptr CBN2 = fg2.eliminate(ord); GaussianBayesNet CBN2 = fg2.eliminate(ord);
CHECK(CBN->equals(*CBN2)); CHECK(assert_equal(CBN,CBN2));
// Base FactorGraph only // Base FactorGraph only
FactorGraph<LinearFactor> fg3(*CBN); FactorGraph<LinearFactor> fg3(CBN);
boost::shared_ptr<BayesNet<ConditionalGaussian> > CBN3 = GaussianBayesNet CBN3 = _eliminate<LinearFactor,ConditionalGaussian>(fg3,ord);
_eliminate<LinearFactor,ConditionalGaussian>(fg3,ord); CHECK(assert_equal(CBN,CBN3));
CHECK(CBN->equals(*CBN3));
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
cpp/testSymbolicBayesNet.cpp
View File

@ -38,9 +38,9 @@ TEST( SymbolicBayesNet, constructor )
// eliminate it // eliminate it
Ordering ordering; Ordering ordering;
ordering += "x2","l1","x1"; ordering += "x2","l1","x1";
SymbolicBayesNet::shared_ptr actual = fg.eliminate(ordering); SymbolicBayesNet actual = fg.eliminate(ordering);
CHECK(assert_equal(expected, *actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
cpp/testSymbolicFactorGraph.cpp
View File

@ -139,9 +139,9 @@ TEST( LinearFactorGraph, eliminate )
// eliminate it // eliminate it
Ordering ordering; Ordering ordering;
ordering += "x2","l1","x1"; ordering += "x2","l1","x1";
SymbolicBayesNet::shared_ptr actual = fg.eliminate(ordering); SymbolicBayesNet actual = fg.eliminate(ordering);
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */