diff --git a/cpp/ConditionalGaussian.cpp b/cpp/ConditionalGaussian.cpp index e361012a5..3f386dba1 100644 --- a/cpp/ConditionalGaussian.cpp +++ b/cpp/ConditionalGaussian.cpp @@ -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& parents, Vector precisions) : - Conditional (key), R_(R),precisions_(precisions), d_(d), parents_(parents) { + const Vector& d, const Matrix& R, const map& 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 diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h index e73294aab..2e3b941c8 100644 --- a/cpp/ConditionalGaussian.h +++ b/cpp/ConditionalGaussian.h @@ -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_); } }; diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index cf4ecd1df..7f73e6993 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -38,8 +38,7 @@ LinearFactor::LinearFactor(const boost::shared_ptr& cg) : } // set sigmas from precisions size_t n = b_.size(); - sigmas_ = ediv(ones(n),cg->get_precisions()); - for(int j=0;jget_sigmas(); } /* ************************************************************************* */ @@ -48,7 +47,7 @@ LinearFactor::LinearFactor(const vector & 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 & factors) const Vector bf = factor->get_b(); for (size_t i=0; isigmas_(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; iinsert(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); } diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 0fe41898b..796b28f8a 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -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("x1"); CHECK(assert_equal(expected,*actual,1e-4)); diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index e4ec30784..4ad19f1f8 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -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; diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 443061c5b..13c881e22 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -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);