diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index a55d40b4c..d71a511e1 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -39,7 +39,7 @@ GaussianFactor::GaussianFactor(const boost::shared_ptr& cg) } // set sigmas from precisions size_t n = b_.size(); - sigmas_ = cg->get_sigmas(); + model_ = noiseModel::Diagonal::Sigmas(cg->get_sigmas()); } /* ************************************************************************* */ @@ -52,7 +52,7 @@ GaussianFactor::GaussianFactor(const vector & factors) size_t m = 0; BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows(); b_ = Vector(m); - sigmas_ = Vector(m); + Vector sigmas(m); size_t pos = 0; // save last position inserted into the new rhs vector @@ -66,8 +66,8 @@ GaussianFactor::GaussianFactor(const vector & factors) const Vector bf = factor->get_b(); for (size_t i=0; isigmas_(i); + // copy the model_ + for (size_t i=0; imodel_->sigma(i); // update the matrices append_factor(factor,m,pos); @@ -75,6 +75,7 @@ GaussianFactor::GaussianFactor(const vector & factors) pos += mf; } if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl; + model_ = noiseModel::Diagonal::Sigmas(sigmas); } /* ************************************************************************* */ @@ -85,7 +86,7 @@ void GaussianFactor::print(const string& s) const { FOREACH_PAIR(j,Aj,As_) gtsam::print(*Aj, "A["+(string)*j+"]=\n"); gtsam::print(b_,"b="); - gtsam::print(sigmas_, "sigmas = "); + model_->print("model"); } } @@ -121,10 +122,7 @@ bool GaussianFactor::equals(const Factor& f, double tol) const { if( !(::equal_with_abs_tol(b_, (lf->b_),tol)) ) return false; - if( !(::equal_with_abs_tol(sigmas_, (lf->sigmas_),tol)) ) - return false; - - return true; + return model_->equals(*(lf->model_),tol); } /* ************************************************************************* */ @@ -138,8 +136,8 @@ Vector GaussianFactor::unweighted_error(const VectorConfig& c) const { /* ************************************************************************* */ Vector GaussianFactor::error_vector(const VectorConfig& c) const { - if (empty()) return (-b_); - return ediv_(unweighted_error(c),sigmas_); + if (empty()) return model_->whiten(-b_); + return model_->whiten(unweighted_error(c)); } /* ************************************************************************* */ @@ -183,12 +181,12 @@ Vector GaussianFactor::operator*(const VectorConfig& x) const { FOREACH_PAIR(j, Aj, As_) Ax += (*Aj * x[*j]); - return ediv_(Ax,sigmas_); + return model_->whiten(Ax); } /* ************************************************************************* */ VectorConfig GaussianFactor::operator^(const Vector& e) const { - Vector E = ediv_(e,sigmas_); + Vector E = model_->whiten(e); VectorConfig x; // Just iterate over all A matrices and insert Ai^e into VectorConfig FOREACH_PAIR(j, Aj, As_) @@ -212,12 +210,7 @@ pair GaussianFactor::matrix(const Ordering& ordering, bool weight Vector b(b_); // divide in sigma so error is indeed 0.5*|Ax-b| - if (weight) { - Vector t = ediv(ones(sigmas_.size()),sigmas_); - A = vector_scale(t, A); - for (int i=0; iWhitenSystem(A,b); return make_pair(A, b); } @@ -254,7 +247,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const { Dimensions::const_iterator it = columnIndices.find(*key); int column_start = it->second; for (size_t i = 0; i < Aj->size1(); i++) { - double sigma_i = sigmas_(i); + double sigma_i = model_->sigma(i); for (size_t j = 0; j < Aj->size2(); j++) if ((*Aj)(i, j) != 0.0) { I.push_back(i + 1); @@ -330,9 +323,9 @@ GaussianFactor::eliminate(const Symbol& key) const // Do in-place QR to get R, d of the augmented system if (verbose) ::print(A,"A"); if (verbose) ::print(b,"b = "); - if (verbose) ::print(sigmas_,"sigmas = "); + if (verbose) model_->print("model"); std::list > solution = - weighted_eliminate(A, b, sigmas_); + weighted_eliminate(A, b, model_->sigmas()); // TODO, fix using NoiseModel // get dimensions of the eliminated variable size_t n1 = getDim(key); @@ -372,7 +365,7 @@ GaussianFactor::eliminate(const Symbol& key) const } // Set sigmas - factor->sigmas_ = sub(newSigmas,n1,maxRank); + factor->model_ = noiseModel::Diagonal::Sigmas(sub(newSigmas,n1,maxRank)); // extract ds vector for the new b factor->set_b(sub(d, n1, maxRank)); @@ -400,7 +393,7 @@ GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const Symbol& key, const Vector b = - unweighted_error(x); // construct factor - shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,sigmas_)); + shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,model_->sigmas())); return factor; } diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 45712c9c7..1c2483c3c 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -17,6 +17,7 @@ #include "Factor.h" #include "Matrix.h" #include "VectorConfig.h" +#include "NoiseModel.h" namespace gtsam { @@ -37,9 +38,9 @@ public: protected: + sharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix std::map As_; // linear matrices Vector b_; // right-hand-side - Vector sigmas_; // vector of standard deviations for each row in the factor public: @@ -49,20 +50,20 @@ public: /** Construct Null factor */ GaussianFactor(const Vector& b_in) : - b_(b_in), sigmas_(ones(b_in.size())){ + b_(b_in) { } /** Construct unary factor */ GaussianFactor(const Symbol& key1, const Matrix& A1, const Vector& b, double sigma) : - b_(b), sigmas_(repeat(b.size(),sigma)) { + b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) { As_.insert(make_pair(key1, A1)); } /** Construct unary factor with vector of sigmas*/ GaussianFactor(const Symbol& key1, const Matrix& A1, const Vector& b, const Vector& sigmas) : - b_(b), sigmas_(sigmas) { + b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) { As_.insert(make_pair(key1, A1)); } @@ -70,7 +71,7 @@ public: GaussianFactor(const Symbol& key1, const Matrix& A1, const Symbol& key2, const Matrix& A2, const Vector& b, double sigma) : - b_(b), sigmas_(repeat(b.size(),sigma)) { + b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) { As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key2, A2)); } @@ -80,7 +81,7 @@ public: const Symbol& key2, const Matrix& A2, const Symbol& key3, const Matrix& A3, const Vector& b, double sigma) : - b_(b), sigmas_(repeat(b.size(),sigma)) { + b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) { As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key2, A2)); As_.insert(make_pair(key3, A3)); @@ -89,7 +90,7 @@ public: /** Construct an n-ary factor */ GaussianFactor(const std::vector > &terms, const Vector &b, double sigma) : - b_(b), sigmas_(repeat(b.size(),sigma)) { + b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) { for(unsigned int i=0; i > &terms, const Vector &b, const Vector& sigmas) : - b_(b), sigmas_(sigmas) { + b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) { for (unsigned int i = 0; i < terms.size(); i++) As_.insert(terms[i]); } @@ -136,7 +137,7 @@ public: const Vector& get_b() const { return b_; } /** get a copy of sigmas */ - const Vector& get_sigmas() const { return sigmas_; } + const Vector& get_sigmas() const { return model_->sigmas(); } /** * get a copy of the A matrix from a specific node