136 lines
3.5 KiB
C++
136 lines
3.5 KiB
C++
/*
|
|
* NoiseModel
|
|
*
|
|
* Created on: Jan 13, 2010
|
|
* Author: Richard Roberts
|
|
* Author: Frank Dellaert
|
|
*/
|
|
|
|
#include <limits>
|
|
#include <iostream>
|
|
#include "NoiseModel.h"
|
|
|
|
namespace ublas = boost::numeric::ublas;
|
|
typedef ublas::matrix_column<Matrix> column;
|
|
|
|
static double inf = std::numeric_limits<double>::infinity();
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
namespace noiseModel {
|
|
|
|
/* ************************************************************************* */
|
|
void Gaussian::print(const string& name) const {
|
|
gtsam::print(sqrt_information_, "Gaussian");
|
|
}
|
|
|
|
bool Gaussian::equals(const Base& m, double tol) const {
|
|
const Gaussian* p = dynamic_cast<const Gaussian*> (&m);
|
|
if (p == NULL) return false;
|
|
return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, tol);
|
|
}
|
|
|
|
Vector Gaussian::whiten(const Vector& v) const {
|
|
return sqrt_information_ * v;
|
|
}
|
|
|
|
Vector Gaussian::unwhiten(const Vector& v) const {
|
|
return backSubstituteUpper(sqrt_information_, v);
|
|
}
|
|
|
|
double Gaussian::Mahalanobis(const Vector& v) const {
|
|
// Note: for Diagonal, which does ediv_, will be correct for constraints
|
|
Vector w = whiten(v);
|
|
return inner_prod(w, w);
|
|
}
|
|
|
|
// functional
|
|
Matrix Gaussian::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 Gaussian::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) :
|
|
Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)),
|
|
sigmas_(sigmas) {
|
|
}
|
|
|
|
void Diagonal::print(const string& name) const {
|
|
gtsam::print(sigmas_, "Diagonal sigmas " + name);
|
|
}
|
|
|
|
Vector Diagonal::whiten(const Vector& v) const {
|
|
return emul(v, invsigmas_);
|
|
}
|
|
|
|
Vector Diagonal::unwhiten(const Vector& v) const {
|
|
return emul(v, sigmas_);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
void Constrained::print(const std::string& name) const {
|
|
gtsam::print(sigmas_, "Constrained sigmas " + name);
|
|
}
|
|
|
|
Vector Constrained::whiten(const Vector& v) const {
|
|
// ediv_ does the right thing with the errors
|
|
return ediv_(v, sigmas_);
|
|
}
|
|
|
|
Matrix Constrained::Whiten(const Matrix& H) const {
|
|
throw logic_error("noiseModel::Constrained cannot Whiten");
|
|
}
|
|
|
|
void Constrained::WhitenInPlace(Matrix& H) const {
|
|
throw logic_error("noiseModel::Constrained cannot Whiten");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
void Isotropic::print(const string& name) const {
|
|
cout << "Isotropic sigma " << name << " " << sigma_ << endl;
|
|
}
|
|
|
|
double Isotropic::Mahalanobis(const Vector& v) const {
|
|
double dot = inner_prod(v, v);
|
|
return dot * invsigma_ * invsigma_;
|
|
}
|
|
|
|
Vector Isotropic::whiten(const Vector& v) const {
|
|
return v * invsigma_;
|
|
}
|
|
|
|
Vector Isotropic::unwhiten(const Vector& v) const {
|
|
return v * sigma_;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Unit::print(const std::string& name) const {
|
|
cout << "Unit " << name << endl;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
}
|
|
} // gtsam
|