74 lines
1.8 KiB
C++
74 lines
1.8 KiB
C++
/*
|
|
* NoiseModel.cpp
|
|
*
|
|
* Created on: Jan 13, 2010
|
|
* Author: Richard Roberts
|
|
* Author: Frank Dellaert
|
|
*/
|
|
|
|
#include "NoiseModel.h"
|
|
|
|
namespace ublas = boost::numeric::ublas;
|
|
typedef ublas::matrix_column<Matrix> column;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
Vector GaussianNoiseModel::whiten(const Vector& v) const {
|
|
return sqrt_information_ * v;
|
|
}
|
|
|
|
Vector GaussianNoiseModel::unwhiten(const Vector& v) const {
|
|
return backSubstituteUpper(sqrt_information_, v);
|
|
}
|
|
|
|
// functional
|
|
Matrix GaussianNoiseModel::Whiten(const Matrix& H) const {
|
|
size_t m = H.size1(), n = H.size2();
|
|
Matrix W(m, n);
|
|
for (int j = 0; j < n; j++) {
|
|
Vector wj = whiten(column(H, j));
|
|
for (int i = 0; i < m; i++)
|
|
W(i, j) = wj(i);
|
|
}
|
|
return W;
|
|
}
|
|
|
|
// in place
|
|
void GaussianNoiseModel::WhitenInPlace(Matrix& H) const {
|
|
size_t m = H.size1(), n = H.size2();
|
|
for (int j = 0; j < n; j++) {
|
|
Vector wj = whiten(column(H, j));
|
|
for (int i = 0; i < m; i++)
|
|
H(i, j) = wj(i);
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// TODO: can we avoid calling reciprocal twice ?
|
|
Diagonal::Diagonal(const Vector& sigmas) :
|
|
GaussianNoiseModel(diag(reciprocal(sigmas))),
|
|
invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) {
|
|
}
|
|
|
|
Vector Diagonal::whiten(const Vector& v) const {
|
|
return emul(v, invsigmas_);
|
|
}
|
|
|
|
Vector Diagonal::unwhiten(const Vector& v) const {
|
|
return emul(v, sigmas_);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
Vector Isotropic::whiten(const Vector& v) const {
|
|
return v * invsigma_;
|
|
}
|
|
|
|
Vector Isotropic::unwhiten(const Vector& v) const {
|
|
return v * sigma_;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
} // gtsam
|