Fixed indentation
Richard Roberts
parent
ed338300a1
commit
ea9b44ea15
690
cpp/NoiseModel.h
690
cpp/NoiseModel.h
|
|
@ -15,387 +15,387 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class SharedDiagonal; // forward declare, defined at end
|
||||
class SharedDiagonal; // forward declare, defined at end
|
||||
|
||||
namespace noiseModel {
|
||||
|
||||
/**
|
||||
* noiseModel::Base is the abstract base class for all noise models.
|
||||
*
|
||||
* Noise models must implement a 'whiten' function to normalize an error vector,
|
||||
* and an 'unwhiten' function to unnormalize an error vector.
|
||||
*/
|
||||
class Base : public Testable<Base> {
|
||||
|
||||
protected:
|
||||
|
||||
size_t dim_;
|
||||
|
||||
public:
|
||||
|
||||
Base(size_t dim):dim_(dim) {}
|
||||
virtual ~Base() {}
|
||||
|
||||
/**
|
||||
* Dimensionality
|
||||
*/
|
||||
inline size_t dim() const { return dim_;}
|
||||
|
||||
/**
|
||||
* Whiten an error vector.
|
||||
*/
|
||||
virtual Vector whiten(const Vector& v) const = 0;
|
||||
|
||||
/**
|
||||
* Unwhiten an error vector.
|
||||
*/
|
||||
virtual Vector unwhiten(const Vector& v) const = 0;
|
||||
|
||||
/** in-place whiten, override if can be done more efficiently */
|
||||
virtual void whitenInPlace(Vector& v) const {
|
||||
v = whiten(v);
|
||||
}
|
||||
|
||||
/** in-place unwhiten, override if can be done more efficiently */
|
||||
virtual void unwhitenInPlace(Vector& v) const {
|
||||
v = unwhiten(v);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Gaussian implements the mathematical model
|
||||
* |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
|
||||
* where
|
||||
* y = whiten(x) = R*x
|
||||
* x = unwhiten(x) = inv(R)*y
|
||||
* as indeed
|
||||
* |y|^2 = y'*y = x'*R'*R*x
|
||||
* Various derived classes are available that are more efficient.
|
||||
*/
|
||||
struct Gaussian: public Base {
|
||||
|
||||
private:
|
||||
|
||||
// TODO: store as boost upper-triangular or whatever is passed from above
|
||||
/* Matrix square root of information matrix (R) */
|
||||
boost::optional<Matrix> sqrt_information_;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
const Matrix& thisR() const {
|
||||
// should never happen
|
||||
if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
|
||||
return *sqrt_information_;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/** protected constructor takes square root information matrix */
|
||||
Gaussian(size_t dim, const boost::optional<Matrix>& sqrt_information = boost::none) :
|
||||
Base(dim), sqrt_information_(sqrt_information) {
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Gaussian> shared_ptr;
|
||||
|
||||
/**
|
||||
* A Gaussian noise model created by specifying a square root information matrix.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr SqrtInformation(const Matrix& R) {
|
||||
return shared_ptr(new Gaussian(R.size1(),R));
|
||||
}
|
||||
|
||||
/**
|
||||
* A Gaussian noise model created by specifying a covariance matrix.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
|
||||
|
||||
/**
|
||||
* A Gaussian noise model created by specifying an information matrix.
|
||||
*/
|
||||
static shared_ptr Information(const Matrix& Q) {
|
||||
return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q)));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual bool equals(const Base& expected, double tol) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
|
||||
/**
|
||||
* Mahalanobis distance v'*R'*R*v = <R*v,R*v>
|
||||
*/
|
||||
virtual double Mahalanobis(const Vector& v) const;
|
||||
|
||||
/**
|
||||
* Multiply a derivative with R (derivative of whiten)
|
||||
* Equivalent to whitening each column of the input matrix.
|
||||
*/
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
|
||||
/**
|
||||
* In-place version
|
||||
*/
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
|
||||
/**
|
||||
* Whiten a system, in place as well
|
||||
*/
|
||||
inline void WhitenSystem(Matrix& A, Vector& b) const {
|
||||
WhitenInPlace(A);
|
||||
whitenInPlace(b);
|
||||
}
|
||||
|
||||
/**
|
||||
* Apply appropriately weighted QR factorization to the system [A b]
|
||||
* Q' * [A b] = [R d]
|
||||
* Dimensions: (r*m) * m*(n+1) = r*(n+1)
|
||||
* @param Ab is the m*(n+1) augmented system matrix [A b]
|
||||
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
|
||||
*/
|
||||
virtual SharedDiagonal QR(Matrix& Ab) const;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
virtual Matrix R() const { return thisR();}
|
||||
|
||||
/**
|
||||
* Simple check for constrained-ness
|
||||
*/
|
||||
virtual bool isConstrained() const {return false;}
|
||||
|
||||
}; // Gaussian
|
||||
|
||||
|
||||
// FD: does not work, ambiguous overload :-(
|
||||
// inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);}
|
||||
|
||||
/**
|
||||
* 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 the static constructor functions Sigmas etc...
|
||||
*/
|
||||
class Diagonal : public Gaussian {
|
||||
protected:
|
||||
|
||||
/** sigmas and reciprocal */
|
||||
Vector sigmas_, invsigmas_;
|
||||
|
||||
/** protected constructor takes sigmas */
|
||||
Diagonal(const Vector& sigmas);
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Diagonal> shared_ptr;
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
|
||||
* standard devations, the diagonal of the square root covariance matrix.
|
||||
*/
|
||||
static shared_ptr Sigmas(const Vector& sigmas, bool smart=false);
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of variances, i.e.
|
||||
* i.e. the diagonal of the covariance matrix.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr Variances(const Vector& variances, bool smart = false);
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of precisions, i.e.
|
||||
* i.e. the diagonal of the information matrix, i.e., weights
|
||||
*/
|
||||
static shared_ptr Precisions(const Vector& precisions) {
|
||||
return Variances(reciprocal(precisions));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
namespace noiseModel {
|
||||
|
||||
/**
|
||||
* Return standard deviations (sqrt of diagonal)
|
||||
*/
|
||||
inline const Vector& sigmas() const { return sigmas_; }
|
||||
inline double sigma(size_t i) const { return sigmas_(i); }
|
||||
|
||||
/**
|
||||
* generate random variate
|
||||
*/
|
||||
virtual Vector sample() const;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
* noiseModel::Base is the abstract base class for all noise models.
|
||||
*
|
||||
* Noise models must implement a 'whiten' function to normalize an error vector,
|
||||
* and an 'unwhiten' function to unnormalize an error vector.
|
||||
*/
|
||||
virtual Matrix R() const {
|
||||
return diag(invsigmas_);
|
||||
}
|
||||
}; // Diagonal
|
||||
class Base : public Testable<Base> {
|
||||
|
||||
/**
|
||||
* A Constrained constrained model is a specialization of Diagonal which allows
|
||||
* some or all of the sigmas to be zero, forcing the error to be zero there.
|
||||
* All other Gaussian models are guaranteed to have a non-singular square-root
|
||||
* information matrix, but this class is specifically equipped to deal with
|
||||
* singular noise models, specifically: whiten will return zero on those
|
||||
* components that have zero sigma *and* zero error, infinity otherwise.
|
||||
*/
|
||||
class Constrained : public Diagonal {
|
||||
protected:
|
||||
protected:
|
||||
|
||||
// Constrained does not have member variables
|
||||
// Instead (possibly zero) sigmas are stored in Diagonal Base class
|
||||
size_t dim_;
|
||||
|
||||
/** protected constructor takes sigmas */
|
||||
Constrained(const Vector& sigmas) :Diagonal(sigmas) {}
|
||||
public:
|
||||
|
||||
public:
|
||||
Base(size_t dim):dim_(dim) {}
|
||||
virtual ~Base() {}
|
||||
|
||||
typedef boost::shared_ptr<Constrained> shared_ptr;
|
||||
/**
|
||||
* Dimensionality
|
||||
*/
|
||||
inline size_t dim() const { return dim_;}
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of
|
||||
* standard devations, some of which might be zero
|
||||
* TODO: make smart - check for zeros
|
||||
*/
|
||||
static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = false) {
|
||||
return shared_ptr(new Constrained(sigmas));
|
||||
}
|
||||
/**
|
||||
* Whiten an error vector.
|
||||
*/
|
||||
virtual Vector whiten(const Vector& v) const = 0;
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of
|
||||
* standard devations, some of which might be zero
|
||||
*/
|
||||
static shared_ptr MixedVariances(const Vector& variances) {
|
||||
return shared_ptr(new Constrained(esqrt(variances)));
|
||||
}
|
||||
/**
|
||||
* Unwhiten an error vector.
|
||||
*/
|
||||
virtual Vector unwhiten(const Vector& v) const = 0;
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of
|
||||
* precisions, some of which might be inf
|
||||
*/
|
||||
static shared_ptr MixedPrecisions(const Vector& precisions) {
|
||||
return MixedVariances(reciprocal(precisions));
|
||||
}
|
||||
/** in-place whiten, override if can be done more efficiently */
|
||||
virtual void whitenInPlace(Vector& v) const {
|
||||
v = whiten(v);
|
||||
}
|
||||
|
||||
/**
|
||||
* Fully constrained. TODO: subclass ?
|
||||
*/
|
||||
static shared_ptr All(size_t dim) {
|
||||
return MixedSigmas(repeat(dim,0));
|
||||
}
|
||||
/** in-place unwhiten, override if can be done more efficiently */
|
||||
virtual void unwhitenInPlace(Vector& v) const {
|
||||
v = unwhiten(v);
|
||||
}
|
||||
};
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
/**
|
||||
* Gaussian implements the mathematical model
|
||||
* |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
|
||||
* where
|
||||
* y = whiten(x) = R*x
|
||||
* x = unwhiten(x) = inv(R)*y
|
||||
* as indeed
|
||||
* |y|^2 = y'*y = x'*R'*R*x
|
||||
* Various derived classes are available that are more efficient.
|
||||
*/
|
||||
struct Gaussian: public Base {
|
||||
|
||||
// Whitening Jacobians does not make sense for possibly constrained
|
||||
// noise model and will throw an exception.
|
||||
private:
|
||||
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
// TODO: store as boost upper-triangular or whatever is passed from above
|
||||
/* Matrix square root of information matrix (R) */
|
||||
boost::optional<Matrix> sqrt_information_;
|
||||
|
||||
/**
|
||||
* Apply QR factorization to the system [A b], taking into account constraints
|
||||
*/
|
||||
virtual SharedDiagonal QR(Matrix& Ab) const;
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
const Matrix& thisR() const {
|
||||
// should never happen
|
||||
if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
|
||||
return *sqrt_information_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check constrained is always true
|
||||
*/
|
||||
virtual bool isConstrained() const {return true;}
|
||||
protected:
|
||||
|
||||
}; // Constrained
|
||||
/** protected constructor takes square root information matrix */
|
||||
Gaussian(size_t dim, const boost::optional<Matrix>& sqrt_information = boost::none) :
|
||||
Base(dim), sqrt_information_(sqrt_information) {
|
||||
}
|
||||
|
||||
/**
|
||||
* An isotropic noise model corresponds to a scaled diagonal covariance
|
||||
* To construct, use one of the static methods
|
||||
*/
|
||||
class Isotropic : public Diagonal {
|
||||
protected:
|
||||
double sigma_, invsigma_;
|
||||
public:
|
||||
|
||||
/** protected constructor takes sigma */
|
||||
Isotropic(size_t dim, double sigma) :
|
||||
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
|
||||
typedef boost::shared_ptr<Gaussian> shared_ptr;
|
||||
|
||||
public:
|
||||
/**
|
||||
* A Gaussian noise model created by specifying a square root information matrix.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr SqrtInformation(const Matrix& R) {
|
||||
return shared_ptr(new Gaussian(R.size1(),R));
|
||||
}
|
||||
|
||||
typedef boost::shared_ptr<Isotropic> shared_ptr;
|
||||
/**
|
||||
* A Gaussian noise model created by specifying a covariance matrix.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a standard devation sigma
|
||||
*/
|
||||
static shared_ptr Sigma(size_t dim, double sigma) {
|
||||
return shared_ptr(new Isotropic(dim, sigma));
|
||||
}
|
||||
/**
|
||||
* A Gaussian noise model created by specifying an information matrix.
|
||||
*/
|
||||
static shared_ptr Information(const Matrix& Q) {
|
||||
return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q)));
|
||||
}
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a variance = sigma^2.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr Variance(size_t dim, double variance, bool smart = false);
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual bool equals(const Base& expected, double tol) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a precision
|
||||
*/
|
||||
static shared_ptr Precision(size_t dim, double precision) {
|
||||
return Variance(dim, 1.0/precision);
|
||||
}
|
||||
/**
|
||||
* Mahalanobis distance v'*R'*R*v = <R*v,R*v>
|
||||
*/
|
||||
virtual double Mahalanobis(const Vector& v) const;
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual double Mahalanobis(const Vector& v) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
/**
|
||||
* Multiply a derivative with R (derivative of whiten)
|
||||
* Equivalent to whitening each column of the input matrix.
|
||||
*/
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
|
||||
/**
|
||||
* Return standard deviation
|
||||
*/
|
||||
inline double sigma() const { return sigma_; }
|
||||
/**
|
||||
* In-place version
|
||||
*/
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
|
||||
/**
|
||||
* generate random variate
|
||||
*/
|
||||
virtual Vector sample() const;
|
||||
/**
|
||||
* Whiten a system, in place as well
|
||||
*/
|
||||
inline void WhitenSystem(Matrix& A, Vector& b) const {
|
||||
WhitenInPlace(A);
|
||||
whitenInPlace(b);
|
||||
}
|
||||
|
||||
};
|
||||
/**
|
||||
* Apply appropriately weighted QR factorization to the system [A b]
|
||||
* Q' * [A b] = [R d]
|
||||
* Dimensions: (r*m) * m*(n+1) = r*(n+1)
|
||||
* @param Ab is the m*(n+1) augmented system matrix [A b]
|
||||
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
|
||||
*/
|
||||
virtual SharedDiagonal QR(Matrix& Ab) const;
|
||||
|
||||
/**
|
||||
* Unit: i.i.d. unit-variance noise on all m dimensions.
|
||||
*/
|
||||
class Unit : public Isotropic {
|
||||
protected:
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
virtual Matrix R() const { return thisR();}
|
||||
|
||||
Unit(size_t dim): Isotropic(dim,1.0) {}
|
||||
/**
|
||||
* Simple check for constrained-ness
|
||||
*/
|
||||
virtual bool isConstrained() const {return false;}
|
||||
|
||||
public:
|
||||
}; // Gaussian
|
||||
|
||||
typedef boost::shared_ptr<Unit> shared_ptr;
|
||||
|
||||
/**
|
||||
* Create a unit covariance noise model
|
||||
*/
|
||||
static shared_ptr Create(size_t dim) {
|
||||
return shared_ptr(new Unit(dim));
|
||||
}
|
||||
// FD: does not work, ambiguous overload :-(
|
||||
// inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); }
|
||||
virtual Vector whiten(const Vector& v) const { return v; }
|
||||
virtual Vector unwhiten(const Vector& v) const { return v; }
|
||||
virtual Matrix Whiten(const Matrix& H) const { return H; }
|
||||
virtual void WhitenInPlace(Matrix& H) const {}
|
||||
};
|
||||
/**
|
||||
* 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 the static constructor functions Sigmas etc...
|
||||
*/
|
||||
class Diagonal : public Gaussian {
|
||||
protected:
|
||||
|
||||
} // namespace noiseModel
|
||||
/** sigmas and reciprocal */
|
||||
Vector sigmas_, invsigmas_;
|
||||
|
||||
using namespace noiseModel;
|
||||
/** protected constructor takes sigmas */
|
||||
Diagonal(const Vector& sigmas);
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Diagonal> shared_ptr;
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
|
||||
* standard devations, the diagonal of the square root covariance matrix.
|
||||
*/
|
||||
static shared_ptr Sigmas(const Vector& sigmas, bool smart=false);
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of variances, i.e.
|
||||
* i.e. the diagonal of the covariance matrix.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr Variances(const Vector& variances, bool smart = false);
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of precisions, i.e.
|
||||
* i.e. the diagonal of the information matrix, i.e., weights
|
||||
*/
|
||||
static shared_ptr Precisions(const Vector& precisions) {
|
||||
return Variances(reciprocal(precisions));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
|
||||
/**
|
||||
* Return standard deviations (sqrt of diagonal)
|
||||
*/
|
||||
inline const Vector& sigmas() const { return sigmas_; }
|
||||
inline double sigma(size_t i) const { return sigmas_(i); }
|
||||
|
||||
/**
|
||||
* generate random variate
|
||||
*/
|
||||
virtual Vector sample() const;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
virtual Matrix R() const {
|
||||
return diag(invsigmas_);
|
||||
}
|
||||
}; // Diagonal
|
||||
|
||||
/**
|
||||
* A Constrained constrained model is a specialization of Diagonal which allows
|
||||
* some or all of the sigmas to be zero, forcing the error to be zero there.
|
||||
* All other Gaussian models are guaranteed to have a non-singular square-root
|
||||
* information matrix, but this class is specifically equipped to deal with
|
||||
* singular noise models, specifically: whiten will return zero on those
|
||||
* components that have zero sigma *and* zero error, infinity otherwise.
|
||||
*/
|
||||
class Constrained : public Diagonal {
|
||||
protected:
|
||||
|
||||
// Constrained does not have member variables
|
||||
// Instead (possibly zero) sigmas are stored in Diagonal Base class
|
||||
|
||||
/** protected constructor takes sigmas */
|
||||
Constrained(const Vector& sigmas) :Diagonal(sigmas) {}
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Constrained> shared_ptr;
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of
|
||||
* standard devations, some of which might be zero
|
||||
* TODO: make smart - check for zeros
|
||||
*/
|
||||
static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = false) {
|
||||
return shared_ptr(new Constrained(sigmas));
|
||||
}
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of
|
||||
* standard devations, some of which might be zero
|
||||
*/
|
||||
static shared_ptr MixedVariances(const Vector& variances) {
|
||||
return shared_ptr(new Constrained(esqrt(variances)));
|
||||
}
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of
|
||||
* precisions, some of which might be inf
|
||||
*/
|
||||
static shared_ptr MixedPrecisions(const Vector& precisions) {
|
||||
return MixedVariances(reciprocal(precisions));
|
||||
}
|
||||
|
||||
/**
|
||||
* Fully constrained. TODO: subclass ?
|
||||
*/
|
||||
static shared_ptr All(size_t dim) {
|
||||
return MixedSigmas(repeat(dim,0));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
|
||||
// Whitening Jacobians does not make sense for possibly constrained
|
||||
// noise model and will throw an exception.
|
||||
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
|
||||
/**
|
||||
* Apply QR factorization to the system [A b], taking into account constraints
|
||||
*/
|
||||
virtual SharedDiagonal QR(Matrix& Ab) const;
|
||||
|
||||
/**
|
||||
* Check constrained is always true
|
||||
*/
|
||||
virtual bool isConstrained() const {return true;}
|
||||
|
||||
}; // Constrained
|
||||
|
||||
/**
|
||||
* An isotropic noise model corresponds to a scaled diagonal covariance
|
||||
* To construct, use one of the static methods
|
||||
*/
|
||||
class Isotropic : public Diagonal {
|
||||
protected:
|
||||
double sigma_, invsigma_;
|
||||
|
||||
/** protected constructor takes sigma */
|
||||
Isotropic(size_t dim, double sigma) :
|
||||
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Isotropic> shared_ptr;
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a standard devation sigma
|
||||
*/
|
||||
static shared_ptr Sigma(size_t dim, double sigma) {
|
||||
return shared_ptr(new Isotropic(dim, sigma));
|
||||
}
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a variance = sigma^2.
|
||||
* @param smart, check if can be simplified to derived class
|
||||
*/
|
||||
static shared_ptr Variance(size_t dim, double variance, bool smart = false);
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a precision
|
||||
*/
|
||||
static shared_ptr Precision(size_t dim, double precision) {
|
||||
return Variance(dim, 1.0/precision);
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual double Mahalanobis(const Vector& v) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
|
||||
/**
|
||||
* Return standard deviation
|
||||
*/
|
||||
inline double sigma() const { return sigma_; }
|
||||
|
||||
/**
|
||||
* generate random variate
|
||||
*/
|
||||
virtual Vector sample() const;
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Unit: i.i.d. unit-variance noise on all m dimensions.
|
||||
*/
|
||||
class Unit : public Isotropic {
|
||||
protected:
|
||||
|
||||
Unit(size_t dim): Isotropic(dim,1.0) {}
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Unit> shared_ptr;
|
||||
|
||||
/**
|
||||
* Create a unit covariance noise model
|
||||
*/
|
||||
static shared_ptr Create(size_t dim) {
|
||||
return shared_ptr(new Unit(dim));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); }
|
||||
virtual Vector whiten(const Vector& v) const { return v; }
|
||||
virtual Vector unwhiten(const Vector& v) const { return v; }
|
||||
virtual Matrix Whiten(const Matrix& H) const { return H; }
|
||||
virtual void WhitenInPlace(Matrix& H) const {}
|
||||
};
|
||||
|
||||
} // namespace noiseModel
|
||||
|
||||
using namespace noiseModel;
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue