129 lines
3.1 KiB
C++
129 lines
3.1 KiB
C++
/*
|
|
* NoiseModel.h
|
|
*
|
|
* Created on: Jan 13, 2010
|
|
* Author: Richard Roberts
|
|
* Author: Frank Dellaert
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "Vector.h"
|
|
#include "Matrix.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* NoiseModel is the abstract base class for all noise models. NoiseModels
|
|
* must implement a 'whiten' function to normalize an error vector, and an
|
|
* 'unwhiten' function to unnormalize an error vector.
|
|
*/
|
|
struct NoiseModel {
|
|
|
|
/**
|
|
* Whiten an error vector.
|
|
*/
|
|
virtual Vector whiten(const Vector& v) const = 0;
|
|
|
|
/**
|
|
* Unwhiten an error vector.
|
|
*/
|
|
virtual Vector unwhiten(const Vector& v) const = 0;
|
|
};
|
|
|
|
/**
|
|
* An isotropic noise model corresponds to a scaled diagonal covariance
|
|
* This class has no public constructors. Instead, use either either the
|
|
* Sigma or Variance class.
|
|
*/
|
|
class Isotropic : public NoiseModel {
|
|
protected:
|
|
double sigma_;
|
|
double invsigma_;
|
|
|
|
Isotropic(double sigma): sigma_(sigma), invsigma_(1.0/sigma) {}
|
|
Isotropic(const Isotropic& isotropic):
|
|
sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {}
|
|
|
|
public:
|
|
Vector whiten(const Vector& v) const;
|
|
Vector unwhiten(const Vector& v) const;
|
|
};
|
|
|
|
/**
|
|
* An isotropic noise model using sigma, the standard deviation.
|
|
*/
|
|
class Sigma : public Isotropic {
|
|
public:
|
|
Sigma(const Sigma& isotropic): Isotropic(isotropic) {}
|
|
Sigma(double sigma): Isotropic(sigma) {}
|
|
};
|
|
|
|
/**
|
|
* An isotropic noise model using the noise variance = sigma^2.
|
|
*/
|
|
class Variance : public Isotropic {
|
|
public:
|
|
Variance(const Variance& v): Isotropic(v) {}
|
|
Variance(double variance): Isotropic(sqrt(variance)) {}
|
|
};
|
|
|
|
/**
|
|
* A diagonal noise model implements a diagonal covariance matrix, with the
|
|
* elements of the diagonal specified in a Vector. This class has no public
|
|
* constructors, instead, use either the Sigmas or Variances class.
|
|
*/
|
|
class Diagonal : public NoiseModel {
|
|
protected:
|
|
Vector sigmas_;
|
|
Vector invsigmas_;
|
|
|
|
Diagonal() {}
|
|
Diagonal(const Vector& sigmas);
|
|
Diagonal(const Diagonal& d);
|
|
|
|
public:
|
|
Vector whiten(const Vector& v) const;
|
|
Vector unwhiten(const Vector& v) const;
|
|
};
|
|
|
|
/**
|
|
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
|
|
* standard devations, i.e. the diagonal of the square root covariance
|
|
* matrix.
|
|
*/
|
|
class Sigmas : public Diagonal {
|
|
public:
|
|
Sigmas(const Sigmas& s): Diagonal(s) {}
|
|
Sigmas(const Vector& sigmas): Diagonal(sigmas) {}
|
|
};
|
|
|
|
/**
|
|
* A diagonal noise model created by specifying a Vector of variances, i.e.
|
|
* i.e. the diagonal of the covariance matrix.
|
|
*/
|
|
class Variances : public Diagonal {
|
|
public:
|
|
Variances(const Variances& s): Diagonal(s) {}
|
|
Variances(const Vector& variances);
|
|
};
|
|
|
|
/**
|
|
* A full covariance noise model.
|
|
*/
|
|
class FullCovariance : public NoiseModel {
|
|
protected:
|
|
Matrix sqrt_covariance_;
|
|
Matrix sqrt_inv_covariance_;
|
|
|
|
public:
|
|
|
|
FullCovariance(const Matrix& covariance);
|
|
FullCovariance(const FullCovariance& c);
|
|
|
|
Vector whiten(const Vector& v) const;
|
|
Vector unwhiten(const Vector& v) const;
|
|
};
|
|
|
|
}
|