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

ConditionalGaussian now stores sigmas

release/4.3a0
Frank Dellaert 2009-11-05 06:59:59 +00:00
parent 88b8f4e16b
commit e87c19ed7a
6 changed files with 58 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/ConditionalGaussian.cpp
View File

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

18
cpp/ConditionalGaussian.h
View File

@ -44,8 +44,8 @@ protected:
/** the names and the matrices connecting to parent nodes */ /** the names and the matrices connecting to parent nodes */
Parents parents_; Parents parents_;
/** vector of precisions */ /** vector of standard deviations */
Vector precisions_; Vector sigmas_;
/** the RHS vector */ /** the RHS vector */
Vector d_; Vector d_;
@ -62,26 +62,26 @@ public:
/** constructor with no parents /** constructor with no parents
* |Rx-d| * |Rx-d|
*/ */
ConditionalGaussian(const std::string& key, Vector d, Matrix R, Vector precisions); ConditionalGaussian(const std::string& key, Vector d, Matrix R, Vector sigmas);
/** constructor with only one parent /** constructor with only one parent
* |Rx+Sy-d| * |Rx+Sy-d|
*/ */
ConditionalGaussian(const std::string& key, Vector d, Matrix R, ConditionalGaussian(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, Vector precisions); const std::string& name1, Matrix S, Vector sigmas);
/** constructor with two parents /** constructor with two parents
* |Rx+Sy+Tz-d| * |Rx+Sy+Tz-d|
*/ */
ConditionalGaussian(const std::string& key, Vector d, Matrix R, ConditionalGaussian(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector precisions); const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas);
/** /**
* constructor with number of arbitrary parents * constructor with number of arbitrary parents
* |Rx+sum(Ai*xi)-d| * |Rx+sum(Ai*xi)-d|
*/ */
ConditionalGaussian(const std::string& key, const Vector& d, ConditionalGaussian(const std::string& key, const Vector& d,
const Matrix& R, const Parents& parents, Vector precisions); const Matrix& R, const Parents& parents, Vector sigmas);
/** deconstructor */ /** deconstructor */
virtual ~ConditionalGaussian() {} virtual ~ConditionalGaussian() {}
@ -101,7 +101,7 @@ public:
/** return stuff contained in ConditionalGaussian */ /** return stuff contained in ConditionalGaussian */
const Vector& get_d() const {return d_;} const Vector& get_d() const {return d_;}
const Matrix& get_R() const {return R_;} const Matrix& get_R() const {return R_;}
const Vector& get_precisions() const {return precisions_;} const Vector& get_sigmas() const {return sigmas_;}
/** STL like, return the iterator pointing to the first node */ /** STL like, return the iterator pointing to the first node */
const_iterator const parentsBegin() const { const_iterator const parentsBegin() const {
@ -123,7 +123,7 @@ public:
return parents_.count(key); return parents_.count(key);
} }
/** /**
* solve a conditional Gaussian - currently just multiplies in the precisions * solve a conditional Gaussian
* @param x configuration in which the parents values (y,z,...) are known * @param x configuration in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...) * @return solution x = R \ (d - Sy - Tz - ...)
*/ */
@ -143,7 +143,7 @@ private:
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(d_); ar & BOOST_SERIALIZATION_NVP(d_);
ar & BOOST_SERIALIZATION_NVP(precisions_); ar & BOOST_SERIALIZATION_NVP(sigmas_);
ar & BOOST_SERIALIZATION_NVP(parents_); ar & BOOST_SERIALIZATION_NVP(parents_);
} }
}; };

31
cpp/LinearFactor.cpp
View File

@ -38,8 +38,7 @@ LinearFactor::LinearFactor(const boost::shared_ptr<ConditionalGaussian>& cg) :
} }
// set sigmas from precisions // set sigmas from precisions
size_t n = b_.size(); size_t n = b_.size();
sigmas_ = ediv(ones(n),cg->get_precisions()); sigmas_ = cg->get_sigmas();
for(int j=0;j<n;j++) sigmas_(j)=sqrt(sigmas_(j));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -48,7 +47,7 @@ LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
bool verbose = false; bool verbose = false;
if (verbose) cout << "LinearFactor::LinearFactor (factors)" << endl; if (verbose) cout << "LinearFactor::LinearFactor (factors)" << endl;
// Create RHS and precision vector of the right size by adding together row counts // Create RHS and sigmas of right size by adding together row counts
size_t m = 0; size_t m = 0;
BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows(); BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows();
b_ = Vector(m); b_ = Vector(m);
@ -66,7 +65,7 @@ LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
const Vector bf = factor->get_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_ // copy the sigmas_
for (size_t i=0; i<mf; i++) sigmas_(pos+i) = factor->sigmas_(i); for (size_t i=0; i<mf; i++) sigmas_(pos+i) = factor->sigmas_(i);
// update the matrices // update the matrices
@ -241,20 +240,17 @@ LinearFactor::eliminate(const string& key)
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key)); ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf); return make_pair(cg,lf);
} }
if (verbose) cout << "<<<<<<<<<<<< 1" << endl;
// create an internal ordering that eliminates key first // create an internal ordering that eliminates key first
Ordering ordering; Ordering ordering;
ordering += key; ordering += key;
BOOST_FOREACH(string k, keys()) BOOST_FOREACH(string k, keys())
if (k != key) ordering += k; if (k != key) ordering += k;
if (verbose) cout << "<<<<<<<<<<<< 2" << endl;
// extract A, b from the combined linear factor (ensure that x is leading) // extract A, b from the combined linear factor (ensure that x is leading)
Matrix A; Vector b; Matrix A; Vector b;
boost::tie(A, b) = matrix(ordering); boost::tie(A, b) = matrix(ordering);
size_t m = A.size1(); size_t n = A.size2(); size_t m = A.size1(); size_t n = A.size2();
if (verbose) cout << "<<<<<<<<<<<< 3" << endl;
// get dimensions of the eliminated variable // get dimensions of the eliminated variable
size_t n1 = getDim(key); size_t n1 = getDim(key);
@ -274,42 +270,34 @@ LinearFactor::eliminate(const string& key)
// copy in b // copy in b
for (int i=0; i<m; ++i) for (int i=0; i<m; ++i)
Rd(i,n) = b(i); Rd(i,n) = b(i);
if (verbose) cout << "<<<<<<<<<<<< 4" << endl;
// Do in-place QR to get R, d of the augmented system // Do in-place QR to get R, d of the augmented system
if (verbose) ::print(Rd,"Rd before"); if (verbose) ::print(Rd,"Rd before");
householder(Rd, maxRank); householder(Rd, maxRank);
if (verbose) ::print(Rd,"Rd after"); if (verbose) ::print(Rd,"Rd after");
if (verbose) cout << "<<<<<<<<<<<< 5" << endl;
// R as calculated by householder has inverse sigma on diagonal // R as calculated by householder has inverse sigma on diagonal
// Use them to normalize R to unit-upper-triangular matrix // Use them to normalize R to unit-upper-triangular matrix
Vector sigmas(m); // standard deviations Vector sigmas(m); // standard deviations
Vector tau(n1); // precisions for conditional
if (verbose) cout << n1 << " " << n << " " << m << endl; if (verbose) cout << n1 << " " << n << " " << m << endl;
for (int i=0; i<maxRank; ++i) { for (int i=0; i<maxRank; ++i) {
double Rii = Rd(i,i); double Rii = Rd(i,i);
// detect rank < maxRank if (fabs(Rii)<1e-8) { maxRank=i; break;} // detect if rank < maxRank
if (fabs(Rii)<1e-8) { maxRank=i; break;} sigmas(i) = 1.0/Rii; // calculate sigma
sigmas(i) = 1.0/Rii; for (int j=0; j<=n; ++j) Rd(i,j) = Rd(i,j)*sigmas(i); // normalize
if (i<n1) tau(i) = Rii*Rii; if (sigmas(i)<0) sigmas(i)=-sigmas(i); // make sure sigma positive
for (int j=0; j<=n; ++j)
Rd(i,j) = Rd(i,j)*sigmas(i);
} }
if (verbose) cout << "<<<<<<<<<<<< 6" << endl;
// extract RHS // extract RHS
Vector d(m); Vector d(m);
for (int i=0; i<m; ++i) for (int i=0; i<m; ++i)
d(i) = Rd(i,n); d(i) = Rd(i,n);
if (verbose) cout << "<<<<<<<<<<<< 7" << endl;
// create base conditional Gaussian // create base conditional Gaussian
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key,
sub(d, 0, n1), // form d vector sub(d, 0, n1), // form d vector
sub(Rd, 0, n1, 0, n1), // form R matrix sub(Rd, 0, n1, 0, n1), // form R matrix
sub(tau, 0, n1))); // get precisions sub(sigmas, 0, n1))); // get standard deviations
if (verbose) cout << "<<<<<<<<<<<< 8" << endl;
// extract the block matrices for parents in both CG and LF // extract the block matrices for parents in both CG and LF
LinearFactor::shared_ptr lf(new LinearFactor); LinearFactor::shared_ptr lf(new LinearFactor);
@ -321,16 +309,13 @@ LinearFactor::eliminate(const string& key)
lf->insert(cur_key, sub(Rd, n1, maxRank, j, j+dim)); lf->insert(cur_key, sub(Rd, n1, maxRank, j, j+dim));
j+=dim; j+=dim;
} }
if (verbose) cout << "<<<<<<<<<<<< 9" << endl;
// Set sigmas // Set sigmas
lf->sigmas_ = sub(sigmas,n1,maxRank); lf->sigmas_ = sub(sigmas,n1,maxRank);
if (verbose) cout << "<<<<<<<<<<<< 10" << endl;
// extract ds vector for the new b // extract ds vector for the new b
lf->set_b(sub(d, n1, maxRank)); lf->set_b(sub(d, n1, maxRank));
if (verbose) lf->print("lf"); if (verbose) lf->print("lf");
if (verbose) cout << "<<<<<<<<<<<< 11" << endl;
return make_pair(cg, lf); return make_pair(cg, lf);
} }

4
cpp/testBayesTree.cpp
View File

@ -131,8 +131,8 @@ TEST( BayesTree, balanced_smoother_marginals )
0.0, 1.0}; 0.0, 1.0};
Matrix R1 = Matrix_(2,2, data1); Matrix R1 = Matrix_(2,2, data1);
Vector d1(2); d1(0) = -0.615385; d1(1) = 0; Vector d1(2); d1(0) = -0.615385; d1(1) = 0;
Vector tau1(2); tau1(0) = 1.61803; tau1(1) = 1.61803; Vector sigma1(2); sigma1(0) = 0.786153; sigma1(1) = 0.786153;
ConditionalGaussian expected("x1",d1, R1, tau1); ConditionalGaussian expected("x1",d1, R1, sigma1);
ConditionalGaussian::shared_ptr actual = bayesTree.marginal<LinearFactor>("x1"); ConditionalGaussian::shared_ptr actual = bayesTree.marginal<LinearFactor>("x1");
CHECK(assert_equal(expected,*actual,1e-4)); CHECK(assert_equal(expected,*actual,1e-4));

32
cpp/testLinearFactor.cpp
View File

@ -329,12 +329,12 @@ TEST( LinearFactor, eliminate )
); );
Vector d(2); d(0) = 0.2; d(1) = -0.14; Vector d(2); d(0) = 0.2; d(1) = -0.14;
Vector tau(2); Vector sigmas(2);
tau(0) = 125.0; sigmas(0) = 1/sqrt(125.0);
tau(1) = 125.0; sigmas(1) = 1/sqrt(125.0);
// Check the conditional Gaussian // Check the conditional Gaussian
ConditionalGaussian expectedCG("x2", d,R11,"l1",S12,"x1",S13,tau); ConditionalGaussian expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas);
// the expected linear factor // the expected linear factor
double sigma = 0.2236; double sigma = 0.2236;
@ -415,18 +415,18 @@ TEST( LinearFactor, eliminate )
// ); // );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525; // Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
// //
// Vector tau(2); // Vector sigmas(2);
// tau(0) = R11(0,0); // sigmas(0) = R11(0,0);
// tau(1) = R11(1,1); // sigmas(1) = R11(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/sigmas(0);
// N(1,1) = 1/tau(1); // N(1,1) = 1/sigmas(1);
// S12 = N*S12; // S12 = N*S12;
// R11 = N*R11; // R11 = N*R11;
// //
// ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,tau); // ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,sigmas);
// //
// // the expected linear factor // // the expected linear factor
// double sigma = 0.2236; // double sigma = 0.2236;
@ -544,18 +544,18 @@ TEST( LinearFactor, matrix )
// ); // );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525; // Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
// //
// Vector tau(2); // Vector sigmas(2);
// tau(0) = R11(0,0); // sigmas(0) = R11(0,0);
// tau(1) = R11(1,1); // sigmas(1) = R11(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/sigmas(0);
// N(1,1) = 1/tau(1); // N(1,1) = 1/sigmas(1);
// S12 = N*S12; // S12 = N*S12;
// R11 = N*R11; // R11 = N*R11;
// //
// ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,tau)); // ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas));
// LinearFactor actualLF(CG); // LinearFactor actualLF(CG);
// // actualLF.print(); // // actualLF.print();
// double precision = 11.1803; // double precision = 11.1803;

24
cpp/testLinearFactorGraph.cpp
View File

@ -206,9 +206,9 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
+0.00,-0.444444 +0.00,-0.444444
); );
Vector d(2); d(0) = -0.133333; d(1) = -0.0222222; Vector d(2); d(0) = -0.133333; d(1) = -0.0222222;
Vector tau(2); tau(0) = 225; tau(1) = 225; Vector sigma(2); sigma(0) = 1./15; sigma(1) = 1./15;
ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,tau); ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -235,9 +235,9 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
+0.0,-0.8 +0.0,-0.8
); );
Vector d(2); d(0) = 0.2; d(1) = -0.14; Vector d(2); d(0) = 0.2; d(1) = -0.14;
Vector tau(2); tau(0) = 125; tau(1) = 125; Vector sigma(2); sigma(0) = 0.0894427; sigma(1) = 0.0894427;
ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,tau); ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -263,9 +263,9 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
+0.0,-0.5 +0.0,-0.5
); );
Vector d(2); d(0) = -0.1; d(1) = 0.25; Vector d(2); d(0) = -0.1; d(1) = 0.25;
Vector tau(2); tau(0) = 50; tau(1) = 50; Vector sigma(2); sigma(0) = 0.141421; sigma(1) = 0.141421;
ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,tau); ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -278,9 +278,9 @@ TEST( LinearFactorGraph, eliminateAll )
0.0, 1.0}; 0.0, 1.0};
Matrix R1 = Matrix_(2,2, data1); Matrix R1 = Matrix_(2,2, data1);
Vector d1(2); d1(0) = -0.1; d1(1) = -0.1; Vector d1(2); d1(0) = -0.1; d1(1) = -0.1;
Vector tau1(2); tau1(0) = 100; tau1(1) = 100; Vector sigma1(2); sigma1(0) = 0.1; sigma1(1) = 0.1;
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1, tau1)); ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1, sigma1));
double data21[] = { 1.0, 0.0, double data21[] = { 1.0, 0.0,
0.0, 1.0}; 0.0, 1.0};
@ -289,9 +289,9 @@ TEST( LinearFactorGraph, eliminateAll )
0.0, -1.0}; 0.0, -1.0};
Matrix A1 = Matrix_(2,2, data22); Matrix A1 = Matrix_(2,2, data22);
Vector d2(2); d2(0) = 0.0; d2(1) = 0.2; Vector d2(2); d2(0) = 0.0; d2(1) = 0.2;
Vector tau2(2); tau2(0) = 45; tau2(1) = 45; Vector sigma2(2); sigma2(0) = 0.149071; sigma2(1) = 0.149071;
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2,"x1", A1,tau2)); ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2,"x1", A1,sigma2));
double data31[] = { 1.0, 0.0, double data31[] = { 1.0, 0.0,
0.0, 1.0}; 0.0, 1.0};
@ -304,9 +304,9 @@ TEST( LinearFactorGraph, eliminateAll )
Matrix A22 = Matrix_(2,2, data33); Matrix A22 = Matrix_(2,2, data33);
Vector d3(2); d3(0) = 0.2; d3(1) = -0.14; Vector d3(2); d3(0) = 0.2; d3(1) = -0.14;
Vector tau3(2); tau3(0) = 125; tau3(1) = 125; Vector sigma3(2); sigma3(0) = 0.0894427; sigma3(1) = 0.0894427;
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3,"l1", A21, "x1", A22, tau3)); ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3,"l1", A21, "x1", A22, sigma3));
GaussianBayesNet expected; GaussianBayesNet expected;
expected.push_back(cg3); expected.push_back(cg3);