84 lines
1.8 KiB
C++
84 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 {
|
|
|
|
// 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);
|
|
}
|
|
}
|
|
|
|
Vector Isotropic::whiten(const Vector& v) const {
|
|
return v * invsigma_;
|
|
}
|
|
|
|
Vector Isotropic::unwhiten(const Vector& v) const {
|
|
return v * sigma_;
|
|
}
|
|
|
|
Diagonal::Diagonal(const Vector& sigmas) :
|
|
GaussianNoiseModel(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(
|
|
sigmas)) {
|
|
}
|
|
|
|
Vector Diagonal::whiten(const Vector& v) const {
|
|
return emul(v, invsigmas_);
|
|
}
|
|
|
|
Vector Diagonal::unwhiten(const Vector& v) const {
|
|
return emul(v, sigmas_);
|
|
}
|
|
|
|
static Vector sqrt(const Vector& v) {
|
|
Vector s(v.size());
|
|
transform(v.begin(), v.end(), s.begin(), ::sqrt);
|
|
return s;
|
|
}
|
|
|
|
Variances::Variances(const Vector& variances) :
|
|
Diagonal(sqrt(variances)) {
|
|
}
|
|
|
|
FullCovariance::FullCovariance(const Matrix& cov) :
|
|
GaussianNoiseModel(cov.size1()),
|
|
sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_(
|
|
inverse_square_root(cov)) {
|
|
}
|
|
|
|
Vector FullCovariance::whiten(const Vector& v) const {
|
|
return sqrt_inv_covariance_ * v;
|
|
}
|
|
|
|
Vector FullCovariance::unwhiten(const Vector& v) const {
|
|
return sqrt_covariance_ * v;
|
|
}
|
|
|
|
} // gtsam
|