diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h
index 8e04bb9a7..a849251ff 100644
--- a/cpp/NoiseModel.h
+++ b/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 {
-
-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 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& sqrt_information = boost::none) :
- Base(dim), sqrt_information_(sqrt_information) {
- }
-
-public:
-
- typedef boost::shared_ptr 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 =
- */
- 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 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 {
-/**
- * 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 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 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& 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 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 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 =
+ */
+ 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 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 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 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 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 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