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;
/* ************************************************************************* */
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, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas),d_(d)
{
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
const string& name1, Matrix S, Vector precisions) :
Conditional (key), R_(R), precisions_(precisions), d_(d) {
const string& name1, Matrix S, Vector sigmas) :
Conditional (key), R_(R), sigmas_(sigmas), 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, Vector precisions) :
Conditional (key), R_(R),precisions_(precisions), d_(d) {
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas), 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, Vector precisions) :
Conditional (key), R_(R),precisions_(precisions), d_(d), parents_(parents) {
const Vector& d, const Matrix& R, const map<string, Matrix>& parents, Vector sigmas) :
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(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
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 sigmas are equal
if (!(::equal_with_abs_tol(sigmas_, p->sigmas_, tol))) return false;
// check if the matrices are the same
// 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 */
Parents parents_;
/** vector of precisions */
Vector precisions_;
/** vector of standard deviations */
Vector sigmas_;
/** the RHS vector */
Vector d_;
@ -62,26 +62,26 @@ public:
/** constructor with no parents
* |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
* |Rx+Sy-d|
*/
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
* |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);
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas);
/**
* 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, Vector precisions);
const Matrix& R, const Parents& parents, Vector sigmas);
/** deconstructor */
virtual ~ConditionalGaussian() {}
@ -101,7 +101,7 @@ public:
/** 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_;}
const Vector& get_sigmas() const {return sigmas_;}
/** STL like, return the iterator pointing to the first node */
const_iterator const parentsBegin() const {
@ -123,7 +123,7 @@ public:
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
* @return solution x = R \ (d - Sy - Tz - ...)
*/
@ -143,7 +143,7 @@ private:
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(sigmas_);
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
size_t n = b_.size();
sigmas_ = ediv(ones(n),cg->get_precisions());
for(int j=0;j<n;j++) sigmas_(j)=sqrt(sigmas_(j));
sigmas_ = cg->get_sigmas();
}
/* ************************************************************************* */
@ -48,7 +47,7 @@ LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
bool verbose = false;
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;
BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows();
b_ = Vector(m);
@ -66,7 +65,7 @@ LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
const Vector bf = factor->get_b();
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);
// update the matrices
@ -241,20 +240,17 @@ LinearFactor::eliminate(const string& key)
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf);
}
if (verbose) cout << "<<<<<<<<<<<< 1" << endl;
// 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;
// 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;
// get dimensions of the eliminated variable
size_t n1 = getDim(key);
@ -274,42 +270,34 @@ LinearFactor::eliminate(const string& key)
// copy in b
for (int i=0; i<m; ++i)
Rd(i,n) = b(i);
if (verbose) cout << "<<<<<<<<<<<< 4" << endl;
// 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;
// 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 (fabs(Rii)<1e-8) { maxRank=i; break;} // detect if rank < maxRank
sigmas(i) = 1.0/Rii; // calculate sigma
for (int j=0; j<=n; ++j) Rd(i,j) = Rd(i,j)*sigmas(i); // normalize
if (sigmas(i)<0) sigmas(i)=-sigmas(i); // make sure sigma positive
}
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;
sub(sigmas, 0, n1))); // get standard deviations
// extract the block matrices for parents in both CG and LF
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));
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);
}

4
cpp/testBayesTree.cpp
View File

@ -131,8 +131,8 @@ TEST( BayesTree, balanced_smoother_marginals )
0.0, 1.0};
Matrix R1 = Matrix_(2,2, data1);
Vector d1(2); d1(0) = -0.615385; d1(1) = 0;
Vector tau1(2); tau1(0) = 1.61803; tau1(1) = 1.61803;
ConditionalGaussian expected("x1",d1, R1, tau1);
Vector sigma1(2); sigma1(0) = 0.786153; sigma1(1) = 0.786153;
ConditionalGaussian expected("x1",d1, R1, sigma1);
ConditionalGaussian::shared_ptr actual = bayesTree.marginal<LinearFactor>("x1");
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 tau(2);
tau(0) = 125.0;
tau(1) = 125.0;
Vector sigmas(2);
sigmas(0) = 1/sqrt(125.0);
sigmas(1) = 1/sqrt(125.0);
// 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
double sigma = 0.2236;
@ -415,18 +415,18 @@ TEST( LinearFactor, eliminate )
// );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
//
// Vector tau(2);
// tau(0) = R11(0,0);
// tau(1) = R11(1,1);
// Vector sigmas(2);
// sigmas(0) = R11(0,0);
// sigmas(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);
// N(0,0) = 1/sigmas(0);
// N(1,1) = 1/sigmas(1);
// S12 = N*S12;
// R11 = N*R11;
//
// ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,tau);
// ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,sigmas);
//
// // the expected linear factor
// double sigma = 0.2236;
@ -544,18 +544,18 @@ TEST( LinearFactor, matrix )
// );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
//
// Vector tau(2);
// tau(0) = R11(0,0);
// tau(1) = R11(1,1);
// Vector sigmas(2);
// sigmas(0) = R11(0,0);
// sigmas(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);
// N(0,0) = 1/sigmas(0);
// N(1,1) = 1/sigmas(1);
// S12 = N*S12;
// 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);
// // actualLF.print();
// double precision = 11.1803;

24
cpp/testLinearFactorGraph.cpp
View File

@ -206,9 +206,9 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
+0.00,-0.444444
);
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));
}
@ -235,9 +235,9 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
+0.0,-0.8
);
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));
}
@ -263,9 +263,9 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
+0.0,-0.5
);
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));
}
@ -278,9 +278,9 @@ TEST( LinearFactorGraph, eliminateAll )
0.0, 1.0};
Matrix R1 = Matrix_(2,2, data1);
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,
0.0, 1.0};
@ -289,9 +289,9 @@ TEST( LinearFactorGraph, eliminateAll )
0.0, -1.0};
Matrix A1 = Matrix_(2,2, data22);
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,
0.0, 1.0};
@ -304,9 +304,9 @@ TEST( LinearFactorGraph, eliminateAll )
Matrix A22 = Matrix_(2,2, data33);
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;
expected.push_back(cg3);