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

Merged r895:900 from branch weightedQR - LinearFactorGraph now has sigmas and ConditionalGaussian now has precisions

release/4.3a0
Richard Roberts 2009-11-04 20:59:16 +00:00
parent 4865edb883
commit e2414561be
21 changed files with 996 additions and 750 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cpp/ConditionalGaussian.cpp
View File

@ -13,29 +13,30 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R) :
Conditional (key), R_(R), d_(d) {
ConditionalGaussian::ConditionalGaussian(const string& key,Vector d, Matrix R, Vector precisions) :
Conditional (key), R_(R),precisions_(precisions),d_(d)
{
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
const string& name1, Matrix S) :
Conditional (key), R_(R), d_(d) {
const string& name1, Matrix S, Vector precisions) :
Conditional (key), R_(R), precisions_(precisions), d_(d) {
parents_.insert(make_pair(name1, S));
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T) :
Conditional (key), R_(R), d_(d) {
const string& name1, Matrix S, const string& name2, Matrix T, Vector precisions) :
Conditional (key), R_(R),precisions_(precisions), d_(d) {
parents_.insert(make_pair(name1, S));
parents_.insert(make_pair(name2, T));
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key,
const Vector& d, const Matrix& R, const map<string, Matrix>& parents) :
Conditional (key), R_(R), d_(d), parents_(parents) {
const Vector& d, const Matrix& R, const map<string, Matrix>& parents, Vector precisions) :
Conditional (key), R_(R),precisions_(precisions), d_(d), parents_(parents) {
}
/* ************************************************************************* */
@ -49,6 +50,7 @@ void ConditionalGaussian::print(const string &s) const
gtsam::print(Aj, "A["+j+"]");
}
gtsam::print(d_,"d");
gtsam::print(precisions_,"precisions");
}
/* ************************************************************************* */
@ -67,6 +69,9 @@ bool ConditionalGaussian::equals(const Conditional &c, double tol) const {
// check if d_ is equal
if (!(::equal_with_abs_tol(d_, p->d_, tol))) return false;
// check if precisions are equal
if (!(::equal_with_abs_tol(precisions_, p->precisions_, tol))) return false;
// check if the matrices are the same
// iterate over the parents_ map
for (it = parents_.begin(); it != parents_.end(); it++) {

216
cpp/ConditionalGaussian.h
View File

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

11
cpp/ConstrainedConditionalGaussian.cpp
View File

@ -20,31 +20,32 @@ ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& v) :
ConditionalGaussian(key, v, eye(v.size())) {
ConditionalGaussian(key, v, eye(v.size()), ones(v.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A) :
ConditionalGaussian(key, b, A) {
ConditionalGaussian(key, b, A, ones(b.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A1,
const std::string& parent, const Matrix& A2) :
ConditionalGaussian(key, b, A1, parent, A2) {
ConditionalGaussian(key, b, A1, parent, A2, ones(b.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A1,
const std::string& parentY, const Matrix& A2, const std::string& parentZ,
const Matrix& A3) :
ConditionalGaussian(key, b, A1, parentY, A2, parentZ, A3) {
ConditionalGaussian(key, b, A1, parentY, A2, parentZ, A3, ones(b.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Matrix& A1,
const std::map<std::string, Matrix>& parents, const Vector& b) :
ConditionalGaussian(key, b, A1, parents) {
ConditionalGaussian(key, b, A1, parents, ones(b.size())*INFINITY) {
}
Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const {

2
cpp/ConstrainedConditionalGaussian.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
* If A1 is triangular, then it can be solved using backsubstitution
* If A1 is invertible, then it can be solved with the Matrix::solve() command
* If A1 overconstrains the system, then ???
* If A1 underconstraints the system, then ???
* If A1 underconstrains the system, then ???
*/
class ConstrainedConditionalGaussian: public ConditionalGaussian {

3
cpp/FactorGraph-inl.h
View File

@ -197,9 +197,12 @@ template<class Factor>
boost::shared_ptr<Factor>
FactorGraph<Factor>::removeAndCombineFactors(const string& key)
{
bool verbose = false;
if (verbose) cout << "FactorGraph::removeAndCombineFactors" << endl;
typedef typename boost::shared_ptr<Factor> shared_factor;
vector<shared_factor> found = findAndRemoveFactors(key);
shared_factor new_factor(new Factor(found));
if (verbose) cout << "FactorGraph::removeAndCombineFactors done" << endl;
return new_factor;
}

235
cpp/LinearFactor.cpp
View File

@ -7,6 +7,8 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/list.hpp> // for operator += in Ordering
#include "Matrix.h"
#include "Ordering.h"
@ -14,52 +16,65 @@
#include "LinearFactor.h"
using namespace std;
using namespace boost::assign;
namespace ublas = boost::numeric::ublas;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
using namespace std;
using namespace gtsam;
typedef pair<const string, Matrix>& mypair;
/* ************************************************************************* */
LinearFactor::LinearFactor(const boost::shared_ptr<ConditionalGaussian>& cg) :
b(cg->get_d()) {
As.insert(make_pair(cg->key(), cg->get_R()));
b_(cg->get_d()) {
As_.insert(make_pair(cg->key(), cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
for (; it != cg->parentsEnd(); it++) {
const std::string& j = it->first;
const Matrix& Aj = it->second;
As.insert(make_pair(j, Aj));
As_.insert(make_pair(j, Aj));
}
// set sigmas from precisions
size_t n = b_.size();
sigmas_ = ediv(ones(n),cg->get_precisions());
for(int j=0;j<n;j++) sigmas_(j)=sqrt(sigmas_(j));
}
/* ************************************************************************* */
LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
{
// Create RHS vector of the right size by adding together row counts
bool verbose = false;
if (verbose) cout << "LinearFactor::LinearFactor (factors)" << endl;
// Create RHS and precision vector of the right size by adding together row counts
size_t m = 0;
BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows();
b = Vector(m);
b_ = Vector(m);
sigmas_ = Vector(m);
size_t pos = 0; // save last position inserted into the new rhs vector
// iterate over all factors
BOOST_FOREACH(shared_ptr factor, factors){
if (verbose) factor->print();
// number of rows for factor f
const size_t mf = factor->numberOfRows();
// copy the rhs vector from factor to b
const Vector bf = factor->get_b();
for (size_t i=0; i<mf; i++) b(pos+i) = bf(i);
for (size_t i=0; i<mf; i++) b_(pos+i) = bf(i);
// copy the precision vector from factor to sigmas_
for (size_t i=0; i<mf; i++) sigmas_(pos+i) = factor->sigmas_(i);
// update the matrices
append_factor(factor,m,pos);
pos += mf;
}
if (verbose) cout << "LinearFactor::LinearFactor done" << endl;
}
/* ************************************************************************* */
@ -68,11 +83,21 @@ void LinearFactor::print(const string& s) const {
if (empty()) cout << " empty" << endl;
else {
string j; Matrix A;
FOREACH_PAIR(j,A,As) gtsam::print(A, "A["+j+"]=\n");
gtsam::print(b,"b=");
FOREACH_PAIR(j,A,As_) gtsam::print(A, "A["+j+"]=\n");
gtsam::print(b_,"b=");
gtsam::print(sigmas_, "sigmas = ");
}
}
/* ************************************************************************* */
size_t LinearFactor::getDim(const std::string& key) const {
const_iterator it = As_.find(key);
if (it != As_.end())
return it->second.size2();
else
return 0;
}
/* ************************************************************************* */
// Check if two linear factors are equal
bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
@ -82,10 +107,10 @@ bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
if (empty()) return (lf->empty());
const_iterator it1 = As.begin(), it2 = lf->As.begin();
if(As.size() != lf->As.size()) return false;
const_iterator it1 = As_.begin(), it2 = lf->As_.begin();
if(As_.size() != lf->As_.size()) return false;
for(; it1 != As.end(); it1++, it2++) {
for(; it1 != As_.end(); it1++, it2++) {
const string& j1 = it1->first, j2 = it2->first;
const Matrix A1 = it1->second, A2 = it2->second;
if (j1 != j2) return false;
@ -93,9 +118,12 @@ bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
return false;
}
if( !(::equal_with_abs_tol(b, (lf->b),tol)) )
if( !(::equal_with_abs_tol(b_, (lf->b_),tol)) )
return false;
if( !(::equal_with_abs_tol(sigmas_, (lf->sigmas_),tol)) )
return false;
return true;
}
@ -103,18 +131,19 @@ bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
// we might have multiple As, so iterate and subtract from b
double LinearFactor::error(const VectorConfig& c) const {
if (empty()) return 0;
Vector e = b;
Vector e = b_;
string j; Matrix Aj;
FOREACH_PAIR(j, Aj, As)
FOREACH_PAIR(j, Aj, As_)
e -= Vector(Aj * c[j]);
return 0.5 * inner_prod(trans(e),e);
Vector weighted = ediv(e,sigmas_);
return 0.5 * inner_prod(weighted,weighted);
}
/* ************************************************************************* */
list<string> LinearFactor::keys() const {
list<string> result;
string j; Matrix A;
FOREACH_PAIR(j,A,As)
FOREACH_PAIR(j,A,As_)
result.push_back(j);
return result;
}
@ -123,7 +152,7 @@ list<string> LinearFactor::keys() const {
VariableSet LinearFactor::variables() const {
VariableSet result;
string j; Matrix A;
FOREACH_PAIR(j,A,As) {
FOREACH_PAIR(j,A,As_) {
Variable v(j,A.size2());
result.insert(v);
}
@ -134,7 +163,7 @@ VariableSet LinearFactor::variables() const {
void LinearFactor::tally_separator(const string& key, set<string>& separator) const {
if(involves(key)) {
string j; Matrix A;
FOREACH_PAIR(j,A,As)
FOREACH_PAIR(j,A,As_)
if(j != key) separator.insert(j);
}
}
@ -149,12 +178,18 @@ pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
matrices.push_back(&Aj);
}
return make_pair( collect(matrices), b);
// divide in sigma so error is indeed 0.5*|Ax-b|
Matrix t = diag(ediv(ones(sigmas_.size()),sigmas_));
Matrix A = t*collect(matrices);
return make_pair(A, t*b_);
}
/* ************************************************************************* */
void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
const size_t pos) {
bool verbose = false;
if (verbose) cout << "LinearFactor::append_factor" << endl;
// iterate over all matrices from the factor f
LinearFactor::const_iterator it = f->begin();
for (; it != f->end(); it++) {
@ -165,8 +200,8 @@ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
const size_t mrhs = A.size1(), n = A.size2();
// find the corresponding matrix among As
const_iterator mine = As.find(j);
const bool exists = mine != As.end();
const_iterator mine = As_.find(j);
const bool exists = mine != As_.end();
// create the matrix or use existing
Matrix Anew = exists ? mine->second : zeros(m, n);
@ -177,9 +212,11 @@ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
Anew(pos + i, j) = A(i, j);
// insert the matrix into the factor
if (exists) As.erase(j);
if (exists) As_.erase(j);
insert(j, Anew);
}
if (verbose) cout << "LinearFactor::append_factor done" << endl;
}
/* ************************************************************************* */
@ -193,71 +230,109 @@ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
pair<ConditionalGaussian::shared_ptr, LinearFactor::shared_ptr>
LinearFactor::eliminate(const string& key)
{
// start empty remaining factor to be returned
boost::shared_ptr<LinearFactor> lf(new LinearFactor);
bool verbose = false;
if (verbose) cout << "LinearFactor::eliminate(" << key << ")" << endl;
// find the matrix associated with key
iterator it = As.find(key);
// if this factor does not involve key, we exit with empty CG and LF
iterator it = As_.find(key);
if (it==As_.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1
LinearFactor::shared_ptr lf(new LinearFactor);
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf);
}
if (verbose) cout << "<<<<<<<<<<<< 1" << endl;
// if this factor does not involve key, we exit with empty CG and LF
if (it==As.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf);
}
// create an internal ordering that eliminates key first
Ordering ordering;
ordering += key;
BOOST_FOREACH(string k, keys())
if (k != key) ordering += k;
if (verbose) cout << "<<<<<<<<<<<< 2" << endl;
// get the matrix reference associated with key
const Matrix& R = it->second;
const size_t m = R.size1(), n = R.size2();
// extract A, b from the combined linear factor (ensure that x is leading)
Matrix A; Vector b;
boost::tie(A, b) = matrix(ordering);
size_t m = A.size1(); size_t n = A.size2();
if (verbose) cout << "<<<<<<<<<<<< 3" << endl;
// if m<n, this factor cannot be eliminated
if (m<n)
throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
// we will apply n Householder reflections to zero out R below diagonal
for(size_t j=0; j < n; j++){
// below, the indices r,c always refer to original A
// get dimensions of the eliminated variable
size_t n1 = getDim(key);
// copy column from matrix to xjm, i.e. x(j:m) = R(j:m,j)
Vector xjm(m-j);
for(size_t r = j ; r < m; r++)
xjm(r-j) = R(r,j);
// calculate the Householder vector v
double beta; Vector vjm;
boost::tie(beta,vjm) = house(xjm);
// if m<n1, this factor cannot be eliminated
size_t maxRank = min(m,n);
if (maxRank<n1)
throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
// update all matrices
BOOST_FOREACH(mypair jA,As) {
// update A matrix using reflection as in householder_
Matrix& A = jA.second;
householder_update(A, j, beta, vjm);
}
// QR performed using an augmented matrix Rd =[A b]
// TODO: We should get rid of this copy
Matrix Rd(m, n+1);
// copy in A
for (int i=0; i<m; ++i)
for (int j=0; j<n; ++j)
Rd(i,j) = A(i,j);
// copy in b
for (int i=0; i<m; ++i)
Rd(i,n) = b(i);
if (verbose) cout << "<<<<<<<<<<<< 4" << endl;
// update RHS, b -= (beta * inner_prod(v,b)) * v;
double inner = 0;
for(size_t r = j ; r < m; r++)
inner += vjm(r-j) * b(r);
for(size_t r = j ; r < m; r++)
b(r) -= beta*inner*vjm(r-j);
} // column j
// create ConditionalGaussian with first n rows
ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(key,::sub(b,0,n), sub(R,0,n,0,n)) );
// create linear factor with remaining rows
lf->set_b(::sub(b,n,m));
// Do in-place QR to get R, d of the augmented system
if (verbose) ::print(Rd,"Rd before");
householder(Rd, maxRank);
if (verbose) ::print(Rd,"Rd after");
if (verbose) cout << "<<<<<<<<<<<< 5" << endl;
// for every separator variable
string j; Matrix A;
FOREACH_PAIR(j,A,As) {
if (j != key) {
const size_t nj = A.size2(); // get dimension of variable
cg->add(j, sub(A,0,n,0,nj)); // add a parent to conditional Gaussian
lf->insert(j,sub(A,n,m,0,nj)); // insert into linear factor
}
}
return make_pair(cg,lf);
// R as calculated by householder has inverse sigma on diagonal
// Use them to normalize R to unit-upper-triangular matrix
Vector sigmas(m); // standard deviations
Vector tau(n1); // precisions for conditional
if (verbose) cout << n1 << " " << n << " " << m << endl;
for (int i=0; i<maxRank; ++i) {
double Rii = Rd(i,i);
// detect rank < maxRank
if (fabs(Rii)<1e-8) { maxRank=i; break;}
sigmas(i) = 1.0/Rii;
if (i<n1) tau(i) = Rii*Rii;
for (int j=0; j<=n; ++j)
Rd(i,j) = Rd(i,j)*sigmas(i);
}
if (verbose) cout << "<<<<<<<<<<<< 6" << endl;
// extract RHS
Vector d(m);
for (int i=0; i<m; ++i)
d(i) = Rd(i,n);
if (verbose) cout << "<<<<<<<<<<<< 7" << endl;
// create base conditional Gaussian
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key,
sub(d, 0, n1), // form d vector
sub(Rd, 0, n1, 0, n1), // form R matrix
sub(tau, 0, n1))); // get precisions
if (verbose) cout << "<<<<<<<<<<<< 8" << endl;
// extract the block matrices for parents in both CG and LF
LinearFactor::shared_ptr lf(new LinearFactor);
size_t j = n1;
BOOST_FOREACH(string cur_key, ordering)
if (cur_key!=key) {
size_t dim = getDim(cur_key);
cg->add(cur_key, sub(Rd, 0, n1, j, j+dim));
lf->insert(cur_key, sub(Rd, n1, maxRank, j, j+dim));
j+=dim;
}
if (verbose) cout << "<<<<<<<<<<<< 9" << endl;
// Set sigmas
lf->sigmas_ = sub(sigmas,n1,maxRank);
if (verbose) cout << "<<<<<<<<<<<< 10" << endl;
// extract ds vector for the new b
lf->set_b(sub(d, n1, maxRank));
if (verbose) lf->print("lf");
if (verbose) cout << "<<<<<<<<<<<< 11" << endl;
return make_pair(cg, lf);
}
/* ************************************************************************* */

88
cpp/LinearFactor.h
View File

@ -35,8 +35,9 @@ public:
protected:
std::map<std::string, Matrix> As; // linear matrices
Vector b; // right-hand-side
std::map<std::string, Matrix> As_; // linear matrices
Vector b_; // right-hand-side
Vector sigmas_; // vector of standard deviations for each row in the factor
public:
@ -46,41 +47,52 @@ public:
/** Construct Null factor */
LinearFactor(const Vector& b_in) :
b(b_in) { //TODO: add a way to initializing base class meaningfully
b_(b_in), sigmas_(ones(b_in.size())){
}
/** Construct unary factor */
LinearFactor(const std::string& key1, const Matrix& A1, const Vector& b_in) :
b(b_in) {
As.insert(make_pair(key1, A1));
LinearFactor(const std::string& key1, const Matrix& A1,
const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
As_.insert(make_pair(key1, A1));
}
/** Construct binary factor */
LinearFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const Vector& b_in) :
b(b_in) {
As.insert(make_pair(key1, A1));
As.insert(make_pair(key2, A2));
const std::string& key2, const Matrix& A2,
const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
}
/** Construct ternary factor */
LinearFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const std::string& key3,
const Matrix& A3, const Vector& b_in) :
b(b_in) {
As.insert(make_pair(key1, A1));
As.insert(make_pair(key2, A2));
As.insert(make_pair(key3, A3));
const std::string& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3,
const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
As_.insert(make_pair(key3, A3));
}
/** Construct an n-ary factor */
LinearFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
const Vector &b_in) :
b(b_in) {
const Vector &b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
for(unsigned int i=0; i<terms.size(); i++)
As.insert(terms[i]);
As_.insert(terms[i]);
}
/** Construct an n-ary factor with a multiple sigmas*/
LinearFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
const Vector &b_in, const Vector& sigmas) :
b_(b_in), sigmas_(sigmas) {
for (unsigned int i = 0; i < terms.size(); i++)
As_.insert(terms[i]);
}
/** Construct from Conditional Gaussian */
LinearFactor(const boost::shared_ptr<ConditionalGaussian>& cg);
@ -97,28 +109,31 @@ public:
// Implementing Factor virtual functions
double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */
std::size_t size() const { return As.size();}
double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
std::size_t size() const { return As_.size();}
/** STL like, return the iterator pointing to the first node */
const_iterator const begin() const { return As.begin();}
const_iterator const begin() const { return As_.begin();}
/** STL like, return the iterator pointing to the last node */
const_iterator const end() const { return As.end(); }
const_iterator const end() const { return As_.end(); }
/** check if empty */
bool empty() const { return b.size() == 0;}
bool empty() const { return b_.size() == 0;}
/** get a copy of b */
const Vector& get_b() const { return b; }
const Vector& get_b() const { return b_; }
/** get a copy of precisions */
const Vector& get_precisions() const { return sigmas_; }
/**
* get a copy of the A matrix from a specific node
* O(log n)
*/
const Matrix& get_A(const std::string& key) const {
const_iterator it = As.find(key);
if (it == As.end())
const_iterator it = As_.find(key);
if (it == As_.end())
throw(std::invalid_argument("LinearFactor::[] invalid key: " + key));
return it->second;
}
@ -130,15 +145,15 @@ public:
/** Check if factor involves variable with key */
bool involves(const std::string& key) const {
const_iterator it = As.find(key);
return (it != As.end());
const_iterator it = As_.find(key);
return (it != As_.end());
}
/**
* return the number of rows from the b vector
* @return a integer with the number of rows from the b vector
*/
int numberOfRows() const { return b.size();}
int numberOfRows() const { return b_.size();}
/**
* Find all variables
@ -152,6 +167,13 @@ public:
*/
VariableSet variables() const;
/**
* Get the dimension of a particular variable
* @param key is the name of the variable
* @return the size of the variable
*/
size_t getDim(const std::string& key) const;
/**
* Add to separator set if this factor involves key, but don't add key itself
* @param key
@ -162,6 +184,7 @@ public:
/**
* Return (dense) matrix associated with factor
* NOTE: in this case, the precisions are baked into A and b
* @param ordering of variables needed for matrix column order
*/
std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
@ -172,12 +195,12 @@ public:
/** insert, copies A */
void insert(const std::string& key, const Matrix& A) {
As.insert(std::make_pair(key, A));
As_.insert(std::make_pair(key, A));
}
/** set RHS, copies b */
void set_b(const Vector& b) {
this->b = b;
this->b_ = b;
}
// set A matrices for the linear factor, same as insert ?
@ -186,6 +209,7 @@ public:
}
/**
* Current Implementation: Full QR factorization
* eliminate (in place!) one of the variables connected to this factor
* @param key the key of the node to be eliminated
* @return a new factor and a conditional gaussian on the eliminated variable

6
cpp/LinearFactorGraph.cpp
View File

@ -145,9 +145,11 @@ LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const {
BOOST_FOREACH(Variable v,vs) {
size_t n = v.dim();
const string& key = v.key();
Matrix A = sigma*eye(n);
//NOTE: this was previously A=sigma*eye(n), when it should be A=eye(n)/sigma
Matrix A = eye(n);
Vector b = zero(n);
shared_factor prior(new LinearFactor(key,A,b));
//To maintain interface with separate sigma, the inverse of the 'sigma' passed in used
shared_factor prior(new LinearFactor(key,A,b, 1/sigma));
result.push_back(prior);
}
return result;

2
cpp/Matrix.cpp
View File

@ -221,7 +221,7 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
v(k) = k<j ? 0.0 : vjm(k-j);
// create Householder reflection matrix Qj = I-beta*v*v'
Matrix Qj = eye(m) - beta * Matrix(outer_prod(v,v));
Matrix Qj = eye(m) - beta * Matrix(outer_prod(v,v)); //BAD: Fix this
R = Qj * R; // update R
Q = Q * Qj; // update Q

2
cpp/Matrix.h
View File

@ -106,7 +106,7 @@ void print(const Matrix& A, const std::string& s = "");
* @param i2 last row index + 1
* @param j1 first col index
* @param j2 last col index + 1
* @return submatrix A(i1:i2,j1:j2)
* @return submatrix A(i1:i2-1,j1:j2-1)
*/
Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);

14
cpp/NonlinearFactor.cpp
View File

@ -39,12 +39,12 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) cons
Vector x1 = c[key_];
// Jacobian A = H(x1)/sigma
Matrix A = H_(x1) / sigma_;
Matrix A = H_(x1);
// Right-hand-side b = error(c) = (z - h(x1))/sigma
Vector b = (z_ - h_(x1)) / sigma_;
Vector b = (z_ - h_(x1));
LinearFactor::shared_ptr p(new LinearFactor(key_, A, b));
LinearFactor::shared_ptr p(new LinearFactor(key_, A, b, sigma_));
return p;
}
@ -93,13 +93,13 @@ LinearFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) cons
Vector x2 = c[key2_];
// Jacobian A = H(x)/sigma
Matrix A1 = H1_(x1, x2) / sigma_;
Matrix A2 = H2_(x1, x2) / sigma_;
Matrix A1 = H1_(x1, x2);
Matrix A2 = H2_(x1, x2);
// Right-hand-side b = (z - h(x))/sigma
Vector b = (z_ - h_(x1, x2)) / sigma_;
Vector b = (z_ - h_(x1, x2));
LinearFactor::shared_ptr p(new LinearFactor(key1_, A1, key2_, A2, b));
LinearFactor::shared_ptr p(new LinearFactor(key1_, A1, key2_, A2, b, sigma_));
return p;
}

21
cpp/Vector.cpp
View File

@ -9,6 +9,7 @@
#include <iostream>
#include <sstream>
#include <iomanip>
#include <boost/FOREACH.hpp>
#ifdef WIN32
#include <Windows.h>
@ -86,14 +87,8 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector zero(size_t n) {
Vector v(n); fill_n(v.begin(),n,0);
return v;
}
/* ************************************************************************* */
Vector ones(size_t n) {
Vector v(n); fill_n(v.begin(),n,1.0);
Vector repeat(size_t n, double value) {
Vector v(n); fill_n(v.begin(),n,value);
return v;
}
@ -146,6 +141,16 @@ namespace gtsam {
return v_return;
}
/* ************************************************************************* */
Vector ediv(const Vector &a, const Vector &b) {
size_t n = a.size();
assert (b.size()==n);
Vector c(n);
for( size_t i = 0; i < n; i++ )
c(i) = a(i)/b(i);
return c;
}
/* ************************************************************************* */
pair<double, Vector > house(Vector &x)
{

32
cpp/Vector.h
View File

@ -35,16 +35,23 @@ Vector Vector_( size_t m, const double* const data);
Vector Vector_(size_t m, ...);
/**
* Create zero vector
* @ param size
* Create vector initialized to a constant value
* @param size
* @param constant value
*/
Vector zero(size_t n);
Vector repeat(size_t n, double value);
/**
* Create zero vector
* @param size
*/
inline Vector zero(size_t n) { return repeat(n,0.0);}
/**
* Create vector initialized to ones
* @ param size
* @param size
*/
Vector ones(size_t n);
inline Vector ones(size_t n) { return repeat(n,1.0);}
/**
* check if all zero
@ -75,13 +82,6 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9)
*/
bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
/**
* vector multiplication by scalar
inline Vector operator*(double s, const Vector& vec ) {
return vec*s;
}
*/
/**
* extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
* @param v Vector
@ -91,6 +91,14 @@ return vec*s;
*/
Vector sub(const Vector &v, size_t i1, size_t i2);
/**
* elementwise division
* @param a first vector
* @param b second vector
* @return vector [a(i)/b(i)]
*/
Vector ediv(const Vector &a, const Vector &b);
/**
* house(x,j) computes HouseHolder vector v and scaling factor beta
* from x, such that the corresponding Householder reflection zeroes out

11
cpp/VectorConfig.cpp
View File

@ -65,22 +65,15 @@ void VectorConfig::print(const std::string& name) const {
/* ************************************************************************* */
bool VectorConfig::equals(const VectorConfig& expected, double tol) const {
string j; Vector vActual;
if( values.size() != expected.size() ) goto fail;
if( values.size() != expected.size() ) return false;
// iterate over all nodes
FOREACH_PAIR(j, vActual, values) {
Vector vExpected = expected[j];
if(!equal_with_abs_tol(vExpected,vActual,tol))
goto fail;
return false;
}
return true;
fail:
// print and return false
print();
expected.print();
return false;
}
/* ************************************************************************* */

64
cpp/smallExample.cpp
View File

@ -129,57 +129,62 @@ LinearFactorGraph createLinearFactorGraph()
// Create
LinearFactorGraph fg;
double sigma1 = 0.1;
// prior on x1
Matrix A11(2,2);
A11(0,0) = 10; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 10;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 1;
Vector b = - c["x1"]/0.1;
Vector b = - c["x1"];
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b));
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b, sigma1));
fg.push_back(f1);
// odometry between x1 and x2
double sigma2 = 0.1;
Matrix A21(2,2);
A21(0,0) = -10 ; A21(0,1) = 0;
A21(1,0) = 0 ; A21(1,1) = -10;
A21(0,0) = -1 ; A21(0,1) = 0;
A21(1,0) = 0 ; A21(1,1) = -1;
Matrix A22(2,2);
A22(0,0) = 10 ; A22(0,1) = 0;
A22(1,0) = 0 ; A22(1,1) = 10;
A22(0,0) = 1 ; A22(0,1) = 0;
A22(1,0) = 0 ; A22(1,1) = 1;
// Vector b(2);
b(0) = 2 ; b(1) = -1;
b(0) = 0.2 ; b(1) = -0.1;
LinearFactor::shared_ptr f2(new LinearFactor("x1", A21, "x2", A22, b));
LinearFactor::shared_ptr f2(new LinearFactor("x1", A21, "x2", A22, b, sigma2));
fg.push_back(f2);
// measurement between x1 and l1
double sigma3 = 0.2;
Matrix A31(2,2);
A31(0,0) = -5; A31(0,1) = 0;
A31(1,0) = 0; A31(1,1) = -5;
A31(0,0) = -1; A31(0,1) = 0;
A31(1,0) = 0; A31(1,1) = -1;
Matrix A32(2,2);
A32(0,0) = 5 ; A32(0,1) = 0;
A32(1,0) = 0 ; A32(1,1) = 5;
A32(0,0) = 1 ; A32(0,1) = 0;
A32(1,0) = 0 ; A32(1,1) = 1;
b(0) = 0 ; b(1) = 1;
b(0) = 0 ; b(1) = 0.2;
LinearFactor::shared_ptr f3(new LinearFactor("x1", A31, "l1", A32, b));
LinearFactor::shared_ptr f3(new LinearFactor("x1", A31, "l1", A32, b, sigma3));
fg.push_back(f3);
// measurement between x2 and l1
double sigma4 = 0.2;
Matrix A41(2,2);
A41(0,0) = -5 ; A41(0,1) = 0;
A41(1,0) = 0 ; A41(1,1) = -5;
A41(0,0) = -1 ; A41(0,1) = 0;
A41(1,0) = 0 ; A41(1,1) = -1;
Matrix A42(2,2);
A42(0,0) = 5 ; A42(0,1) = 0;
A42(1,0) = 0 ; A42(1,1) = 5;
A42(0,0) = 1 ; A42(0,1) = 0;
A42(1,0) = 0 ; A42(1,1) = 1;
b(0)= -1 ; b(1) = 1.5;
b(0)= -0.2 ; b(1) = 0.3;
LinearFactor::shared_ptr f4(new LinearFactor("x2", A41, "l1", A42, b));
LinearFactor::shared_ptr f4(new LinearFactor("x2", A41, "l1", A42, b, sigma4));
fg.push_back(f4);
return fg;
@ -197,11 +202,12 @@ GaussianBayesNet createSmallGaussianBayesNet()
Matrix R22 = Matrix_(1,1,1.0);
Vector d1(1), d2(1);
d1(0) = 9; d2(0) = 5;
Vector tau(1); tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last)
ConditionalGaussian::shared_ptr
Px_y(new ConditionalGaussian("x",d1,R11,"y",S12)),
Py(new ConditionalGaussian("y",d2,R22));
Px_y(new ConditionalGaussian("x",d1,R11,"y",S12,tau)),
Py(new ConditionalGaussian("y",d2,R22,tau));
GaussianBayesNet cbn;
cbn.push_back(Px_y);
cbn.push_back(Py);
@ -276,11 +282,11 @@ ConstrainedLinearFactorGraph createSingleConstraintGraph() {
// create unary factor
// prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1;
Matrix Ax = eye(2) / sigma;
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1 / sigma));
LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1, sigma));
// create binary constraint factor
// between "x" and "y", that is going to be the only factor on "y"
@ -306,9 +312,9 @@ ConstrainedLinearFactorGraph createSingleConstraintGraph() {
ConstrainedLinearFactorGraph createMultiConstraintGraph() {
// unary factor 1
double sigma = 0.1;
Matrix A = eye(2) / sigma;
Vector b = Vector_(2, -2.0, 2.0)/sigma;
LinearFactor::shared_ptr lf1(new LinearFactor("x", A, b));
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
LinearFactor::shared_ptr lf1(new LinearFactor("x", A, b, sigma));
// constraint 1
Matrix A11(2,2);

41
cpp/testConditionalGaussian.cpp
View File

@ -20,11 +20,11 @@
using namespace gtsam;
/* ************************************************************************* */
/* untit test for equals */
/* unit test for equals */
/* ************************************************************************* */
TEST( ConditionalGaussian, equals )
{
// create a conditional gaussion node
// create a conditional gaussian node
Matrix A1(2,2);
A1(0,0) = 1 ; A1(1,0) = 2;
A1(0,1) = 3 ; A1(1,1) = 4;
@ -37,47 +37,52 @@ TEST( ConditionalGaussian, equals )
R(0,0) = 0.1 ; R(1,0) = 0.3;
R(0,1) = 0.0 ; R(1,1) = 0.34;
Vector tau(2);
tau(0) = 1.0;
tau(1) = 0.34;
Vector d(2);
d(0) = 0.2; d(1) = 0.5;
ConditionalGaussian
expected("x",d, R, "x1", A1, "l1", A2),
actual("x",d, R, "x1", A1, "l1", A2);
expected("x",d, R, "x1", A1, "l1", A2, tau),
actual("x",d, R, "x1", A1, "l1", A2, tau);
CHECK( expected.equals(actual) );
}
/* ************************************************************************* */
/* untit test for solve */
/* unit test for solve */
/* ************************************************************************* */
TEST( ConditionalGaussian, solve )
{
//expected solution
Vector expected(2);
expected(0) = 15.0471 ; expected(1) = -18.8824;
expected(0) = 20-3-11 ; expected(1) = 40-7-15;
// create a conditional gaussion node
Matrix R = Matrix_(2,2, 1., 0.,
0., 1.);
Matrix A1 = Matrix_(2,2, 1., 2.,
3., 4.);
3., 4.);
Matrix A2 = Matrix_(2,2, 6., 0.2,
8., 0.4);
Matrix R = Matrix_(2,2, 0.1, 0.3,
0.0, 0.34);
Matrix A2 = Matrix_(2,2, 5., 6.,
7., 8.);
Vector d(2);
d(0) = 0.2; d(1) = 0.5;
d(0) = 20.0; d(1) = 40.0;
ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2);
Vector tau = ones(2);
ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2, tau);
Vector sx1(2);
sx1(0) = 0.2; sx1(1) = 0.5;
sx1(0) = 1.0; sx1(1) = 1.0;
Vector sl1(2);
sl1(0) = 0.5; sl1(1) = 0.8;
sl1(0) = 1.0; sl1(1) = 1.0;
VectorConfig solution;
solution.insert("x1", sx1);
@ -85,7 +90,7 @@ TEST( ConditionalGaussian, solve )
Vector result = cg.solve(solution);
CHECK( equal_with_abs_tol(expected , result, 0.0001));
CHECK(assert_equal(expected , result, 0.0001));
}

18
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -51,7 +51,8 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
Ap(1, 0) = -2.0; Ap(1, 1) = 1.0;
Ap = 33.3333 * Ap;
Vector bp = Vector_(2, 0.0, -10.0);
LinearFactor expectedLF("y", Ap, bp);
double sigma1 = 1;
LinearFactor expectedLF("y", Ap, bp,sigma1);
CHECK(expectedLF.equals(*(fg[0]), 1e-4));
// eliminate y
@ -65,7 +66,16 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
R(0, 0) = 74.5356; R(0, 1) = -59.6285;
R(1, 0) = 0.0; R(1, 1) = 44.7214;
Vector br = Vector_(2, 8.9443, 4.4721);
ConditionalGaussian expected2("y",br, R);
Vector tau(2);
tau(0) = R(0,0);
tau(1) = R(1,1);
// normalize the existing matrices
Matrix N = eye(2,2);
N(0,0) = 1/tau(0);
N(1,1) = 1/tau(1);
R = N*R;
ConditionalGaussian expected2("y",br, R, tau);
CHECK(expected2.equals(*((*cbn)["y"])));
}
@ -120,8 +130,8 @@ TEST( ConstrainedLinearFactorGraph, is_constrained )
// create simple graph
Vector b = Vector_(2, 0.0, 0.0);
LinearFactor::shared_ptr f1(new LinearFactor("x", eye(2), "y", eye(2), b));
LinearFactor::shared_ptr f2(new LinearFactor("z", eye(2), "w", eye(2), b));
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));
LinearConstraint::shared_ptr f3(new LinearConstraint("y", eye(2), "z", eye(2), b));
fg.push_back(f1);
fg.push_back(f2);

4
cpp/testGaussianBayesNet.cpp
View File

@ -34,9 +34,11 @@ TEST( GaussianBayesNet, constructor )
Matrix R22 = Matrix_(1,1,1.0);
Vector d1(1), d2(1);
d1(0) = 9; d2(0) = 5;
Vector tau(1);
tau(0) = 1.;
// define nodes and specify in reverse topological sort (i.e. parents last)
ConditionalGaussian x("x",d1,R11,"y",S12), y("y",d2,R22);
ConditionalGaussian x("x",d1,R11,"y",S12, tau), y("y",d2,R22, tau);
// check small example which uses constructor
GaussianBayesNet cbn = createSmallGaussianBayesNet();

686
cpp/testLinearFactor.cpp
View File

@ -23,40 +23,42 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( LinearFactor, linearFactor )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
{
double sigma = 0.1;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
LinearFactor expected("x1", A1, "x2", A2, b);
Matrix A1(2,2);
A1(0,0) = -1.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -1.0;
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 1.0;
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
Vector b(2);
b(0) = 0.2 ; b(1) = -0.1;
// check if the two factors are the same
CHECK(assert_equal(expected,*lf));
LinearFactor expected("x1", A1, "x2", A2, b, sigma);
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
// check if the two factors are the same
CHECK(assert_equal(expected,*lf));
}
/* ************************************************************************* */
TEST( LinearFactor, keys )
{
// get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr lf = fg[1];
list<string> expected;
expected.push_back("x1");
expected.push_back("x2");
CHECK(lf->keys() == expected);
// get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr lf = fg[1];
list<string> expected;
expected.push_back("x1");
expected.push_back("x2");
CHECK(lf->keys() == expected);
}
/* ************************************************************************* */
@ -66,77 +68,108 @@ TEST( LinearFactor, variables )
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr lf = fg[1];
VariableSet vs = lf->variables();
//TODO: test this
}
/* ************************************************************************* */
TEST( LinearFactor, linearFactor2 )
TEST( LinearFactor, getDim )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// get a factor
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr factor = fg[0];
// get two factors from it and insert the factors into a set
vector<LinearFactor::shared_ptr> lfg;
lfg.push_back(fg[4 - 1]);
lfg.push_back(fg[2 - 1]);
// get the size of a variable
size_t actual = factor->getDim("x1");
// combine in a factor
LinearFactor combined(lfg);
// verify
size_t expected = 2;
CHECK(actual == expected);
}
// the expected combined linear factor
Matrix Ax2 = Matrix_(4, 2, // x2
-5., 0.,
+0., -5.,
10., 0.,
+0., 10.);
/* ************************************************************************* */
TEST( LinearFactor, combine )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
Matrix Al1 = Matrix_(4, 2, // l1
5., 0.,
0., 5.,
// get two factors from it and insert the factors into a vector
vector<LinearFactor::shared_ptr> lfg;
lfg.push_back(fg[4 - 1]);
lfg.push_back(fg[2 - 1]);
// combine in a factor
LinearFactor combined(lfg);
// sigmas
double sigma2 = 0.1;
double sigma4 = 0.2;
Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2);
// the expected combined linear factor
Matrix Ax2 = Matrix_(4, 2, // x2
-1., 0.,
+0., -1.,
1., 0.,
+0., 1.);
Matrix Al1 = Matrix_(4, 2, // l1
1., 0.,
0., 1.,
0., 0.,
0., 0.);
Matrix Ax1 = Matrix_(4, 2, // x1
0.00, 0., // f4
0.00, 0., // f4
-10., 0., // f2
0.00, -10. // f2
);
-1., 0., // f2
0.00, -1. // f2
);
// the RHS
Vector b2(4);
b2(0) = -1;
b2(1) = 1.5;
b2(2) = 2;
b2(3) = -1;
// the RHS
Vector b2(4);
b2(0) = -0.2;
b2(1) = 0.3;
b2(2) = 0.2;
b2(3) = -0.1;
LinearFactor expected("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
CHECK(assert_equal(expected,combined));
// use general constructor for making arbitrary factors
vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1));
LinearFactor expected(meas, b2, sigmas);
CHECK(assert_equal(expected,combined));
}
/* ************************************************************************* */
TEST( NonlinearFactorGraph, linearFactor3){
TEST( NonlinearFactorGraph, combine2){
double sigma1 = 0.0957;
Matrix A11(2,2);
A11(0,0) = 10.4545; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 10.4545;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 1;
Vector b(2);
b(0) = 2 ; b(1) = -1;
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b));
b(0) = 2; b(1) = -1;
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b*sigma1, sigma1));
A11(0,0) = 2; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -2;
double sigma2 = 0.5;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 4 ; b(1) = -5;
LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b));
LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b*sigma2, sigma2));
A11(0,0) = 4; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -4;
double sigma3 = 0.25;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 3 ; b(1) = -88;
LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b));
LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b*sigma3, sigma3));
// TODO: find a real sigma value for this example
double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7;
b(0) = 5 ; b(1) = -6;
LinearFactor::shared_ptr f4(new LinearFactor("x1", A11, b));
LinearFactor::shared_ptr f4(new LinearFactor("x1", A11*sigma4, b*sigma4, sigma4));
vector<LinearFactor::shared_ptr> lfg;
lfg.push_back(f1);
@ -145,20 +178,23 @@ TEST( NonlinearFactorGraph, linearFactor3){
lfg.push_back(f4);
LinearFactor combined(lfg);
Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4);
Matrix A22(8,2);
A22(0,0) = 10.4545; A22(0,1) = 0;
A22(1,0) = 0; A22(1,1) = 10.4545;
A22(2,0) = 2; A22(2,1) = 0;
A22(3,0) = 0; A22(3,1) = -2;
A22(4,0) = 4; A22(4,1) = 0;
A22(5,0) = 0; A22(5,1) = -4;
A22(6,0) = 6; A22(6,1) = 0;
A22(7,0) = 0; A22(7,1) = 7;
A22(0,0) = 1; A22(0,1) = 0;
A22(1,0) = 0; A22(1,1) = 1;
A22(2,0) = 1; A22(2,1) = 0;
A22(3,0) = 0; A22(3,1) = -1;
A22(4,0) = 1; A22(4,1) = 0;
A22(5,0) = 0; A22(5,1) = -1;
A22(6,0) = 0.6; A22(6,1) = 0;
A22(7,0) = 0; A22(7,1) = 0.7;
Vector exb(8);
exb(0) = 2 ; exb(1) = -1; exb(2) = 4 ; exb(3) = -5;
exb(4) = 3 ; exb(5) = -88; exb(6) = 5 ; exb(7) = -6;
LinearFactor expected("x1", A22, exb);
exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2;
exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4;
vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("x1", A22));
LinearFactor expected(meas, exb, sigmas);
CHECK(assert_equal(expected,combined));
}
@ -169,7 +205,7 @@ TEST( LinearFactor, linearFactorN){
1.0, 0.0,
0.0, 1.0),
Vector_(2,
10.0, 5.0))));
10.0, 5.0), 1)));
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x1", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
@ -177,7 +213,7 @@ TEST( LinearFactor, linearFactorN){
10.0, 0.0,
0.0, 10.0),
Vector_(2,
1.0, -2.0))));
1.0, -2.0), 1)));
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x2", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
@ -185,7 +221,7 @@ TEST( LinearFactor, linearFactorN){
10.0, 0.0,
0.0, 10.0),
Vector_(2,
1.5, -1.5))));
1.5, -1.5), 1)));
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x3", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
@ -193,7 +229,7 @@ TEST( LinearFactor, linearFactorN){
10.0, 0.0,
0.0, 10.0),
Vector_(2,
2.0, -1.0))));
2.0, -1.0), 1)));
LinearFactor combinedFactor(f);
@ -237,273 +273,297 @@ TEST( LinearFactor, linearFactorN){
Vector b = Vector_(8,
10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
LinearFactor expected(combinedMeasurement, b);
LinearFactor expected(combinedMeasurement, b, 1.);
CHECK(combinedFactor.equals(expected));
}
/* ************************************************************************* */
TEST( LinearFactor, error )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// get the first factor from the factor graph
LinearFactor::shared_ptr lf = fg[0];
// get the first factor from the factor graph
LinearFactor::shared_ptr lf = fg[0];
// check the error of the first factor with nosiy config
VectorConfig cfg = createZeroDelta();
// check the error of the first factor with noisy config
VectorConfig cfg = createZeroDelta();
// calculate the error from the factor "f1"
// note the error is the same as in testNonlinearFactor
double actual = lf->error(cfg);
DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
// calculate the error from the factor "f1"
// note the error is the same as in testNonlinearFactor
double actual = lf->error(cfg);
DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
}
/* ************************************************************************* */
TEST( LinearFactor, eliminate )
{
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
// x2
-5., 0.,
+0.,-5.,
10., 0.,
+0.,10.
);
Matrix Al1 = Matrix_(4,2,
// l1
5., 0.,
0., 5.,
0., 0.,
0., 0.
);
Matrix Ax1 = Matrix_(4,2,
// x1
0.00, 0., // f4
0.00, 0., // f4
-10., 0., // f2
0.00,-10. // f2
);
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// the RHS
Vector b2(4);
b2(0) = -1;
b2(1) = 1.5;
b2(2) = 2;
b2(3) = -1;
LinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
// get two factors from it and insert the factors into a vector
vector<LinearFactor::shared_ptr> lfg;
lfg.push_back(fg[4 - 1]);
lfg.push_back(fg[2 - 1]);
// eliminate the combined factor
// combine in a factor
LinearFactor combined(lfg);
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
11.1803, 0.00,
0.00, 11.1803
);
Matrix S12 = Matrix_(2,2,
-2.23607, 0.00,
+0.00,-2.23607
);
Matrix S13 = Matrix_(2,2,
-8.94427, 0.00,
+0.00,-8.94427
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian expectedCG("x2",d,R11,"l1",S12,"x1",S13);
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
1.0, 0.0,
0.0, 1.0
);
Matrix S12 = Matrix_(2,2,
-0.2, 0.0,
+0.0,-0.2
);
Matrix S13 = Matrix_(2,2,
-0.8, 0.0,
+0.0,-0.8
);
Vector d(2); d(0) = 0.2; d(1) = -0.14;
// the expected linear factor
Matrix Bl1 = Matrix_(2,2,
// l1
4.47214, 0.00,
0.00, 4.47214
);
Matrix Bx1 = Matrix_(2,2,
// x1
-4.47214, 0.00,
+0.00, -4.47214
);
Vector tau(2);
tau(0) = 125.0;
tau(1) = 125.0;
// the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
LinearFactor expectedLF("l1", Bl1, "x1", Bx1, b1);
// Check the conditional Gaussian
ConditionalGaussian expectedCG("x2", d,R11,"l1",S12,"x1",S13,tau);
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
CHECK(assert_equal(expectedLF,*actualLF,1e-5));
// the expected linear factor
double sigma = 0.2236;
Matrix Bl1 = Matrix_(2,2,
// l1
1.00, 0.00,
0.00, 1.00
);
Matrix Bx1 = Matrix_(2,2,
// x1
-1.00, 0.00,
+0.00, -1.00
);
// the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.2;
LinearFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma);
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
CHECK(assert_equal(expectedLF,*actualLF,1e-5));
}
/* ************************************************************************* */
TEST( LinearFactor, eliminate2 )
{
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
// x2
-5., 0.,
+0.,-5.,
10., 0.,
+0.,10.
);
Matrix Al1x1 = Matrix_(4,4,
// l1 x1
5., 0., 0.00, 0., // f4
0., 5., 0.00, 0., // f4
0., 0., -10., 0., // f2
0., 0., 0.00,-10. // f2
);
// the RHS
Vector b2(4);
b2(0) = -1;
b2(1) = 1.5;
b2(2) = 2;
b2(3) = -1;
LinearFactor combined("x2", Ax2, "l1x1", Al1x1, b2);
// eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
11.1803, 0.00,
0.00, 11.1803
);
Matrix S12 = Matrix_(2,4,
-2.23607, 0.00,-8.94427, 0.00,
+0.00,-2.23607,+0.00,-8.94427
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12);
// the expected linear factor
Matrix Bl1x1 = Matrix_(2,4,
// l1 x1
4.47214, 0.00, -4.47214, 0.00,
0.00, 4.47214, +0.00, -4.47214
);
// the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
LinearFactor expectedLF("l1x1", Bl1x1, b1);
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
CHECK(assert_equal(expectedLF,*actualLF,1e-5));
}
//* ************************************************************************* */
TEST( LinearFactor, default_error )
{
LinearFactor f;
VectorConfig c;
double actual = f.error(c);
CHECK(actual==0.0);
}
//* ************************************************************************* */
TEST( LinearFactor, eliminate_empty )
{
// create an empty factor
LinearFactor f;
// eliminate the empty factor
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = f.eliminate("x2");
// expected Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian expectedCG("x2");
// expected remaining factor is still empty :-)
LinearFactor expectedLF;
// check if the result matches
CHECK(actualCG->equals(expectedCG));
CHECK(actualLF->equals(expectedLF));
}
//* ************************************************************************* */
TEST( LinearFactor, empty )
{
// create an empty factor
LinearFactor f;
CHECK(f.empty()==true);
}
///* ************************************************************************* */
//TEST( LinearFactor, eliminate2 )
//{
// cout << "TEST( LinearFactor, eliminate2 )" << endl;
// // sigmas
// double sigma1 = 0.2;
// double sigma2 = 0.1;
// Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
//
// // the combined linear factor
// Matrix Ax2 = Matrix_(4,2,
// // x2
// -1., 0.,
// +0.,-1.,
// 1., 0.,
// +0.,1.
// );
//
// Matrix Al1x1 = Matrix_(4,4,
// // l1 x1
// 1., 0., 0.00, 0., // f4
// 0., 1., 0.00, 0., // f4
// 0., 0., -1., 0., // f2
// 0., 0., 0.00,-1. // f2
// );
//
// // the RHS
// Vector b2(4);
// b2(0) = -1*sigma1;
// b2(1) = 1.5*sigma1;
// b2(2) = 2*sigma2;
// b2(3) = -1*sigma2;
//
// vector<pair<string, Matrix> > meas;
// meas.push_back(make_pair("x2", Ax2));
// meas.push_back(make_pair("l1x1", Al1x1));
// LinearFactor combined(meas, b2, sigmas);
//
// // eliminate the combined factor
// ConditionalGaussian::shared_ptr actualCG;
// LinearFactor::shared_ptr actualLF;
// boost::tie(actualCG,actualLF) = combined.eliminate("x2");
//
// // create expected Conditional Gaussian
// Matrix R11 = Matrix_(2,2,
// 11.1803, 0.00,
// 0.00, 11.1803
// );
// Matrix S12 = Matrix_(2,4,
// -2.23607, 0.00,-8.94427, 0.00,
// +0.00,-2.23607,+0.00,-8.94427
// );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
//
// Vector tau(2);
// tau(0) = R11(0,0);
// tau(1) = R11(1,1);
//
// // normalize the existing matrices
// Matrix N = eye(2,2);
// N(0,0) = 1/tau(0);
// N(1,1) = 1/tau(1);
// S12 = N*S12;
// R11 = N*R11;
//
// ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,tau);
//
// // the expected linear factor
// double sigma = 0.2236;
// Matrix Bl1x1 = Matrix_(2,4,
// // l1 x1
// 1.00, 0.00, -1.00, 0.00,
// 0.00, 1.00, +0.00, -1.00
// );
//
// // the RHS
// Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
//
// LinearFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma);
//
// // check if the result matches
// CHECK(assert_equal(expectedCG,*actualCG,1e-4)); //FAILS
// CHECK(assert_equal(expectedLF,*actualLF,1e-5)); //FAILS
//}
//
////* ************************************************************************* */
//TEST( LinearFactor, default_error )
//{
// cout << "TEST( LinearFactor, default_error )" << endl;
// LinearFactor f;
// VectorConfig c;
// double actual = f.error(c);
// CHECK(actual==0.0);
//}
//
////* ************************************************************************* */
//TEST( LinearFactor, eliminate_empty )
//{
// cout << "TEST( LinearFactor, eliminate_empty )" << endl;
// // create an empty factor
// LinearFactor f;
//
// // eliminate the empty factor
// ConditionalGaussian::shared_ptr actualCG;
// LinearFactor::shared_ptr actualLF;
// boost::tie(actualCG,actualLF) = f.eliminate("x2");
//
// // expected Conditional Gaussian is just a parent-less node with P(x)=1
// ConditionalGaussian expectedCG("x2");
//
// // expected remaining factor is still empty :-)
// LinearFactor expectedLF;
//
// // check if the result matches
// CHECK(actualCG->equals(expectedCG));
// CHECK(actualLF->equals(expectedLF));
//}
//
////* ************************************************************************* */
//TEST( LinearFactor, empty )
//{
// cout << "TEST( LinearFactor, empty )" << endl;
// // create an empty factor
// LinearFactor f;
// CHECK(f.empty()==true);
//}
//
/* ************************************************************************* */
TEST( LinearFactor, matrix )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
// render with a given ordering
Ordering ord;
ord += "x1","x2";
Matrix A; Vector b;
boost::tie(A,b) = lf->matrix(ord);
Matrix A; Vector b;
boost::tie(A,b) = lf->matrix(ord);
Matrix A1 = Matrix_(2,4,
-10.0, 0.0, 10.0, 0.0,
000.0,-10.0, 0.0, 10.0 );
Vector b1 = Vector_(2, 2.0, -1.0);
Matrix A1 = Matrix_(2,4,
-10.0, 0.0, 10.0, 0.0,
000.0,-10.0, 0.0, 10.0 );
Vector b1 = Vector_(2, 2.0, -1.0);
EQUALITY(A,A1);
EQUALITY(A,A1);
EQUALITY(b,b1);
}
/* ************************************************************************* */
TEST( LinearFactor, size )
{
// create a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// get some factors from the graph
boost::shared_ptr<LinearFactor> factor1 = fg[0];
boost::shared_ptr<LinearFactor> factor2 = fg[1];
boost::shared_ptr<LinearFactor> factor3 = fg[2];
CHECK(factor1->size() == 1);
CHECK(factor2->size() == 2);
CHECK(factor3->size() == 2);
}
/* ************************************************************************* */
TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
{
Matrix R11 = Matrix_(2,2,
11.1803, 0.00,
0.00, 11.1803
);
Matrix S12 = Matrix_(2,2,
-2.23607, 0.00,
+0.00,-2.23607
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12) );
LinearFactor actualLF(CG);
// actualLF.print();
LinearFactor expectedLF("x2",R11,"l1x1",S12,d);
CHECK(assert_equal(expectedLF,actualLF));
}
///* ************************************************************************* */
//TEST( LinearFactor, size )
//{
// cout << "TEST( LinearFactor, size )" << endl;
// // create a linear factor graph
// LinearFactorGraph fg = createLinearFactorGraph();
//
// // get some factors from the graph
// boost::shared_ptr<LinearFactor> factor1 = fg[0];
// boost::shared_ptr<LinearFactor> factor2 = fg[1];
// boost::shared_ptr<LinearFactor> factor3 = fg[2];
//
// CHECK(factor1->size() == 1);
// CHECK(factor2->size() == 2);
// CHECK(factor3->size() == 2);
//}
//
///* ************************************************************************* */
//TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
//{
// cout << "TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )" << endl;
// Matrix R11 = Matrix_(2,2,
// 11.1803, 0.00,
// 0.00, 11.1803
// );
// Matrix S12 = Matrix_(2,2,
// -2.23607, 0.00,
// +0.00,-2.23607
// );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
//
// Vector tau(2);
// tau(0) = R11(0,0);
// tau(1) = R11(1,1);
//
// // normalize the existing matrices
// Matrix N = eye(2,2);
// N(0,0) = 1/tau(0);
// N(1,1) = 1/tau(1);
// S12 = N*S12;
// R11 = N*R11;
//
// ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,tau));
// LinearFactor actualLF(CG);
// // actualLF.print();
// double precision = 11.1803;
// double sigma = 1/sqrt(precision);
// LinearFactor expectedLF("x2",R11,"l1x1",S12,d, sigma);
//
// CHECK(assert_equal(expectedLF,actualLF,1e-5)); //FAILS Precisions are incorrect
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

258
cpp/testLinearFactorGraph.cpp
View File

@ -70,7 +70,13 @@ TEST( LinearFactorGraph, combine_factors_x1 )
// create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// combine all factors
// create sigmas
double sigma1 = 0.1;
double sigma2 = 0.1;
double sigma3 = 0.2;
Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3);
// combine all factors
LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
// the expected linear factor
@ -79,38 +85,43 @@ TEST( LinearFactorGraph, combine_factors_x1 )
0., 0.,
0., 0.,
0., 0.,
5., 0.,
0., 5.
1., 0.,
0., 1.
);
Matrix Ax1 = Matrix_(6,2,
10., 0.,
0.00, 10.,
-10., 0.,
0.00,-10.,
-5., 0.,
00., -5.
1., 0.,
0.00, 1.,
-1., 0.,
0.00,-1.,
-1., 0.,
00., -1.
);
Matrix Ax2 = Matrix_(6,2,
0., 0.,
0., 0.,
10., 0.,
+0.,10.,
1., 0.,
+0.,1.,
0., 0.,
0., 0.
);
// the expected RHS vector
Vector b(6);
b(0) = -1;
b(1) = -1;
b(2) = 2;
b(3) = -1;
b(4) = 0;
b(5) = 1;
b(0) = -1*sigma1;
b(1) = -1*sigma1;
b(2) = 2*sigma2;
b(3) = -1*sigma2;
b(4) = 0*sigma3;
b(5) = 1*sigma3;
LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2));
LinearFactor expected(meas, b, sigmas);
//LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
// check if the two factors are the same
CHECK(assert_equal(expected,*actual));
@ -122,6 +133,11 @@ TEST( LinearFactorGraph, combine_factors_x2 )
// create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// determine sigmas
double sigma1 = 0.1;
double sigma2 = 0.2;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// combine all factors
LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x2");
@ -130,34 +146,38 @@ TEST( LinearFactorGraph, combine_factors_x2 )
// l1
0., 0.,
0., 0.,
5., 0.,
0., 5.
1., 0.,
0., 1.
);
Matrix Ax1 = Matrix_(4,2,
// x1
-10., 0., // f2
0.00,-10., // f2
-1., 0., // f2
0.00,-1., // f2
0.00, 0., // f4
0.00, 0. // f4
);
Matrix Ax2 = Matrix_(4,2,
// x2
10., 0.,
+0.,10.,
-5., 0.,
+0.,-5.
1., 0.,
+0.,1.,
-1., 0.,
+0.,-1.
);
// the expected RHS vector
Vector b(4);
b(0) = 2;
b(1) = -1;
b(2) = -1;
b(3) = 1.5;
b(0) = 2*sigma1;
b(1) = -1*sigma1;
b(2) = -1*sigma2;
b(3) = 1.5*sigma2;
LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2));
LinearFactor expected(meas, b, sigmas);
// check if the two factors are the same
CHECK(assert_equal(expected,*actual));
@ -173,19 +193,21 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
15.0, 00.0,
00.0, 15.0
1.0, 0.0,
0.0, 1.0
);
Matrix S12 = Matrix_(2,2,
-1.66667, 0.00,
+0.00,-1.66667
-0.111111, 0.00,
+0.00,-0.111111
);
Matrix S13 = Matrix_(2,2,
-6.66667, 0.00,
+0.00,-6.66667
-0.444444, 0.00,
+0.00,-0.444444
);
Vector d(2); d(0) = -2; d(1) = -1.0/3.0;
ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13);
Vector d(2); d(0) = -0.133333; d(1) = -0.0222222;
Vector tau(2); tau(0) = 225; tau(1) = 225;
ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,tau);
CHECK(assert_equal(expected,*actual,tol));
}
@ -200,19 +222,21 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
11.1803, 0.00,
0.00, 11.1803
1.0, 0.0,
0.0, 1.0
);
Matrix S12 = Matrix_(2,2,
-2.23607, 0.00,
+0.00,-2.23607
-0.2, 0.0,
+0.0,-0.2
);
Matrix S13 = Matrix_(2,2,
-8.94427, 0.00,
+0.00,-8.94427
-0.8, 0.0,
+0.0,-0.8
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13);
Vector d(2); d(0) = 0.2; d(1) = -0.14;
Vector tau(2); tau(0) = 125; tau(1) = 125;
ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,tau);
CHECK(assert_equal(expected,*actual,tol));
}
@ -226,19 +250,21 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
7.07107, 0.00,
0.00, 7.07107
1.0, 0.0,
0.0, 1.0
);
Matrix S12 = Matrix_(2,2,
-3.53553, 0.00,
+0.00,-3.53553
-0.5, 0.0,
+0.0,-0.5
);
Matrix S13 = Matrix_(2,2,
-3.53553, 0.00,
+0.00,-3.53553
-0.5, 0.0,
+0.0,-0.5
);
Vector d(2); d(0) = -0.707107; d(1) = 1.76777;
ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13);
Vector d(2); d(0) = -0.1; d(1) = 0.25;
Vector tau(2); tau(0) = 50; tau(1) = 50;
ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,tau);
CHECK(assert_equal(expected,*actual,tol));
}
@ -247,39 +273,45 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
TEST( LinearFactorGraph, eliminateAll )
{
// create expected Chordal bayes Net
double data1[] = { 10, 0.0,
0.0, 10};
double data1[] = { 1.0, 0.0,
0.0, 1.0};
Matrix R1 = Matrix_(2,2, data1);
Vector d1(2); d1(0) = -1; d1(1) = -1;
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1));
double data21[] = { 6.7082, 0.0,
0.0, 6.7082};
Vector d1(2); d1(0) = -0.1; d1(1) = -0.1;
Vector tau1(2); tau1(0) = 100; tau1(1) = 100;
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1, tau1));
double data21[] = { 1.0, 0.0,
0.0, 1.0};
Matrix R2 = Matrix_(2,2, data21);
double data22[] = { -6.7082, 0.0,
0.0, -6.7082};
double data22[] = { -1.0, 0.0,
0.0, -1.0};
Matrix A1 = Matrix_(2,2, data22);
Vector d2(2); d2(0) = 0.0; d2(1) = 1.34164;
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2, "x1", A1));
double data31[] = { 11.1803, 0.0,
0.0, 11.1803};
Vector d2(2); d2(0) = 0.0; d2(1) = 0.2;
Vector tau2(2); tau2(0) = 45; tau2(1) = 45;
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2,"x1", A1,tau2));
double data31[] = { 1.0, 0.0,
0.0, 1.0};
Matrix R3 = Matrix_(2,2, data31);
double data32[] = { -2.23607, 0.0,
0.0, -2.23607};
double data32[] = { -0.2, 0.0,
0.0, -0.2};
Matrix A21 = Matrix_(2,2, data32);
double data33[] = { -8.94427, 0.0,
0.0, -8.94427};
double data33[] = { -0.8, 0.0,
0.0, -0.8};
Matrix A22 = Matrix_(2,2, data33);
Vector d3(2); d3(0) = 2.23607; d3(1) = -1.56525;
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3, "l1", A21, "x1", A22));
Vector d3(2); d3(0) = 0.2; d3(1) = -0.14;
Vector tau3(2); tau3(0) = 125; tau3(1) = 125;
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3,"l1", A21, "x1", A22, tau3));
GaussianBayesNet expected;
expected.push_back(cg3);
expected.push_back(cg2);
expected.push_back(cg1);
// Check one ordering
LinearFactorGraph fg1 = createLinearFactorGraph();
Ordering ord1;
@ -294,12 +326,13 @@ TEST( LinearFactorGraph, add_priors )
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactorGraph actual = fg.add_priors(3);
LinearFactorGraph expected = createLinearFactorGraph();
Matrix A = 3*eye(2);
Matrix A = eye(2);
Vector b = zero(2);
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b)));
CHECK(assert_equal(expected,actual));
double sigma = 1.0/3.0;
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b,sigma)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b,sigma)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b,sigma)));
CHECK(assert_equal(expected,actual)); // Fails
}
/* ************************************************************************* */
@ -355,7 +388,6 @@ TEST( LinearFactorGraph, matrix )
/* ************************************************************************* */
TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{
LinearFactorGraph fg = createLinearFactorGraph();
// render with a given ordering
@ -364,7 +396,7 @@ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet )
GaussianBayesNet::shared_ptr CBN = fg.eliminate(ord);
LinearFactorGraph fg2(*CBN);
GaussianBayesNet::shared_ptr CBN2 = fg2.eliminate(ord);
CHECK(CBN->equals(*CBN2));
}
@ -399,39 +431,39 @@ TEST( LinearFactorGraph, OPTIMIZE )
/* ************************************************************************* */
TEST( LinearFactorGraph, COMBINE_GRAPHS_INPLACE)
{
// create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph();
// create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph();
// get sizes
int size1 = fg1.size();
int size2 = fg2.size();
// combine them
fg1.combine(fg2);
CHECK(size1+size2 == fg1.size());
// create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph();
// create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph();
// get sizes
int size1 = fg1.size();
int size2 = fg2.size();
// combine them
fg1.combine(fg2);
CHECK(size1+size2 == fg1.size());
}
/* ************************************************************************* */
TEST( LinearFactorGraph, COMBINE_GRAPHS)
{
// create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph();
// create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph();
// get sizes
int size1 = fg1.size();
int size2 = fg2.size();
// combine them
LinearFactorGraph fg3 = LinearFactorGraph::combine2(fg1, fg2);
CHECK(size1+size2 == fg3.size());
// create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph();
// create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph();
// get sizes
int size1 = fg1.size();
int size2 = fg2.size();
// combine them
LinearFactorGraph fg3 = LinearFactorGraph::combine2(fg1, fg2);
CHECK(size1+size2 == fg3.size());
}
/* ************************************************************************* */

11
cpp/testVector.cpp
View File

@ -192,6 +192,17 @@ TEST( TestVector, whouse_subs_vector2 )
CHECK(assert_equal(pseudo1, expected, 1e-4));
}
/* ************************************************************************* */
TEST( TestVector, ediv )
{
Vector a(3); a(0) = 10; a(1) = 20; a(2) = 30;
Vector b(3); b(0) = 2; b(1) = 5; b(2) = 6;
Vector actual(ediv(a,b));
Vector c(3); c(0) = 5; c(1) = 4; c(2) = 5;
CHECK(assert_equal(c,actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */