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

GaussianFactor now uses noiseModel internally - not yet externally

release/4.3a0
Frank Dellaert 2010-01-19 04:39:28 +00:00
parent 81aaceb890
commit b01c111a1d
2 changed files with 27 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
cpp/GaussianFactor.cpp
View File

@ -39,7 +39,7 @@ GaussianFactor::GaussianFactor(const boost::shared_ptr<GaussianConditional>& cg)
} }
// set sigmas from precisions // set sigmas from precisions
size_t n = b_.size(); size_t n = b_.size();
sigmas_ = cg->get_sigmas(); model_ = noiseModel::Diagonal::Sigmas(cg->get_sigmas());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -52,7 +52,7 @@ GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
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);
sigmas_ = Vector(m); Vector sigmas(m);
size_t pos = 0; // save last position inserted into the new rhs vector size_t pos = 0; // save last position inserted into the new rhs vector
@ -66,8 +66,8 @@ GaussianFactor::GaussianFactor(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 sigmas_ // copy the model_
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->model_->sigma(i);
// update the matrices // update the matrices
append_factor(factor,m,pos); append_factor(factor,m,pos);
@ -75,6 +75,7 @@ GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
pos += mf; pos += mf;
} }
if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl; 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_) FOREACH_PAIR(j,Aj,As_)
gtsam::print(*Aj, "A["+(string)*j+"]=\n"); gtsam::print(*Aj, "A["+(string)*j+"]=\n");
gtsam::print(b_,"b="); gtsam::print(b_,"b=");
gtsam::print(sigmas_, "sigmas = "); model_->print("model");
} }
} }
@ -121,10 +122,7 @@ bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
if( !(::equal_with_abs_tol(b_, (lf->b_),tol)) ) if( !(::equal_with_abs_tol(b_, (lf->b_),tol)) )
return false; return false;
if( !(::equal_with_abs_tol(sigmas_, (lf->sigmas_),tol)) ) return model_->equals(*(lf->model_),tol);
return false;
return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -138,8 +136,8 @@ Vector GaussianFactor::unweighted_error(const VectorConfig& c) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianFactor::error_vector(const VectorConfig& c) const { Vector GaussianFactor::error_vector(const VectorConfig& c) const {
if (empty()) return (-b_); if (empty()) return model_->whiten(-b_);
return ediv_(unweighted_error(c),sigmas_); return model_->whiten(unweighted_error(c));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -183,12 +181,12 @@ Vector GaussianFactor::operator*(const VectorConfig& x) const {
FOREACH_PAIR(j, Aj, As_) FOREACH_PAIR(j, Aj, As_)
Ax += (*Aj * x[*j]); Ax += (*Aj * x[*j]);
return ediv_(Ax,sigmas_); return model_->whiten(Ax);
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig GaussianFactor::operator^(const Vector& e) const { VectorConfig GaussianFactor::operator^(const Vector& e) const {
Vector E = ediv_(e,sigmas_); Vector E = model_->whiten(e);
VectorConfig x; VectorConfig x;
// Just iterate over all A matrices and insert Ai^e into VectorConfig // Just iterate over all A matrices and insert Ai^e into VectorConfig
FOREACH_PAIR(j, Aj, As_) FOREACH_PAIR(j, Aj, As_)
@ -212,12 +210,7 @@ pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight
Vector b(b_); Vector b(b_);
// divide in sigma so error is indeed 0.5*|Ax-b| // divide in sigma so error is indeed 0.5*|Ax-b|
if (weight) { if (weight) model_->WhitenSystem(A,b);
Vector t = ediv(ones(sigmas_.size()),sigmas_);
A = vector_scale(t, A);
for (int i=0; i<b_.size(); ++i)
b(i) *= t(i);
}
return make_pair(A, b); return make_pair(A, b);
} }
@ -254,7 +247,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
Dimensions::const_iterator it = columnIndices.find(*key); Dimensions::const_iterator it = columnIndices.find(*key);
int column_start = it->second; int column_start = it->second;
for (size_t i = 0; i < Aj->size1(); i++) { 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++) for (size_t j = 0; j < Aj->size2(); j++)
if ((*Aj)(i, j) != 0.0) { if ((*Aj)(i, j) != 0.0) {
I.push_back(i + 1); 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 // Do in-place QR to get R, d of the augmented system
if (verbose) ::print(A,"A"); if (verbose) ::print(A,"A");
if (verbose) ::print(b,"b = "); if (verbose) ::print(b,"b = ");
if (verbose) ::print(sigmas_,"sigmas = "); if (verbose) model_->print("model");
std::list<boost::tuple<Vector, double, double> > solution = std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A, b, sigmas_); weighted_eliminate(A, b, model_->sigmas()); // TODO, fix using NoiseModel
// get dimensions of the eliminated variable // get dimensions of the eliminated variable
size_t n1 = getDim(key); size_t n1 = getDim(key);
@ -372,7 +365,7 @@ GaussianFactor::eliminate(const Symbol& key) const
} }
// Set sigmas // Set sigmas
factor->sigmas_ = sub(newSigmas,n1,maxRank); factor->model_ = noiseModel::Diagonal::Sigmas(sub(newSigmas,n1,maxRank));
// extract ds vector for the new b // extract ds vector for the new b
factor->set_b(sub(d, n1, maxRank)); 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); Vector b = - unweighted_error(x);
// construct factor // 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; return factor;
} }

19
cpp/GaussianFactor.h
View File

@ -17,6 +17,7 @@
#include "Factor.h" #include "Factor.h"
#include "Matrix.h" #include "Matrix.h"
#include "VectorConfig.h" #include "VectorConfig.h"
#include "NoiseModel.h"
namespace gtsam { namespace gtsam {
@ -37,9 +38,9 @@ public:
protected: protected:
sharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
std::map<Symbol, Matrix> As_; // linear matrices std::map<Symbol, Matrix> As_; // linear matrices
Vector b_; // right-hand-side Vector b_; // right-hand-side
Vector sigmas_; // vector of standard deviations for each row in the factor
public: public:
@ -49,20 +50,20 @@ public:
/** Construct Null factor */ /** Construct Null factor */
GaussianFactor(const Vector& b_in) : GaussianFactor(const Vector& b_in) :
b_(b_in), sigmas_(ones(b_in.size())){ b_(b_in) {
} }
/** Construct unary factor */ /** Construct unary factor */
GaussianFactor(const Symbol& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, double sigma) : 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(key1, A1));
} }
/** Construct unary factor with vector of sigmas*/ /** Construct unary factor with vector of sigmas*/
GaussianFactor(const Symbol& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, const Vector& sigmas) : const Vector& b, const Vector& sigmas) :
b_(b), sigmas_(sigmas) { b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key1, A1));
} }
@ -70,7 +71,7 @@ public:
GaussianFactor(const Symbol& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2, const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) : 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(key1, A1));
As_.insert(make_pair(key2, A2)); As_.insert(make_pair(key2, A2));
} }
@ -80,7 +81,7 @@ public:
const Symbol& key2, const Matrix& A2, const Symbol& key2, const Matrix& A2,
const Symbol& key3, const Matrix& A3, const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) : 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(key1, A1));
As_.insert(make_pair(key2, A2)); As_.insert(make_pair(key2, A2));
As_.insert(make_pair(key3, A3)); As_.insert(make_pair(key3, A3));
@ -89,7 +90,7 @@ public:
/** Construct an n-ary factor */ /** Construct an n-ary factor */
GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms, GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, double sigma) : 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.size(); i++) for(unsigned int i=0; i<terms.size(); i++)
As_.insert(terms[i]); As_.insert(terms[i]);
} }
@ -97,7 +98,7 @@ public:
/** Construct an n-ary factor with a multiple sigmas*/ /** Construct an n-ary factor with a multiple sigmas*/
GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms, GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, const Vector& sigmas) : 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++) for (unsigned int i = 0; i < terms.size(); i++)
As_.insert(terms[i]); As_.insert(terms[i]);
} }
@ -136,7 +137,7 @@ public:
const Vector& get_b() const { return b_; } const Vector& get_b() const { return b_; }
/** get a copy of sigmas */ /** 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 * get a copy of the A matrix from a specific node