From 87f7e05c15b79d3a87fa824db511e4c4946a8982 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2010 15:10:10 +0000 Subject: [PATCH] Identity model --- cpp/NoiseModel.cpp | 44 +++++++++++++++++++++---------------- cpp/NoiseModel.h | 49 +++++++++++++++++++++++++++++++----------- cpp/testNoiseModel.cpp | 4 ++++ 3 files changed, 67 insertions(+), 30 deletions(-) diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index 3d6bcb673..eb7637246 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -13,9 +13,10 @@ typedef ublas::matrix_column column; namespace gtsam { + // functional Matrix GaussianNoiseModel::Whiten(const Matrix& H) const { - size_t n = H.size2(), m = H.size1(); - Matrix W = zeros(m, n); + 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++) @@ -24,6 +25,16 @@ namespace gtsam { 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_; } @@ -33,11 +44,8 @@ namespace gtsam { } Diagonal::Diagonal(const Vector& sigmas) : - sigmas_(sigmas), invsigmas_(1.0 / sigmas) { - } - - Diagonal::Diagonal(const Diagonal& d) : - sigmas_(d.sigmas_), invsigmas_(d.invsigmas_) { + GaussianNoiseModel(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal( + sigmas)) { } Vector Diagonal::whiten(const Vector& v) const { @@ -48,20 +56,20 @@ namespace gtsam { return emul(v, sigmas_); } - Variances::Variances(const Vector& variances) { - sigmas_.resize(variances.size()); - std::transform(variances.begin(), variances.end(), sigmas_.begin(), sqrt); - invsigmas_ = reciprocal(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) : - sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_( - inverse_square_root(cov)) { - } - - FullCovariance::FullCovariance(const FullCovariance& cov) : - sqrt_covariance_(cov.sqrt_covariance_), sqrt_inv_covariance_( - cov.sqrt_inv_covariance_) { + GaussianNoiseModel(cov.size1()), + sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_( + inverse_square_root(cov)) { } Vector FullCovariance::whiten(const Vector& v) const { diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h index f061c34bc..5658a4619 100644 --- a/cpp/NoiseModel.h +++ b/cpp/NoiseModel.h @@ -21,7 +21,15 @@ namespace gtsam { * It must implement a 'whiten' function to normalize an error vector, and an * 'unwhiten' function to unnormalize an error vector. */ - struct NoiseModel /* TODO : public Testable */ { + class NoiseModel /* TODO : public Testable */ { + + protected: + + size_t dim_; + + public: + + NoiseModel(size_t dim):dim_(dim) {} /** * Whiten an error vector. @@ -46,6 +54,8 @@ namespace gtsam { */ struct GaussianNoiseModel : public NoiseModel { + GaussianNoiseModel(size_t dim):NoiseModel(dim) {} + /** * Return R itself, but note that Whiten(H) is cheaper than R*H */ @@ -56,6 +66,11 @@ namespace gtsam { * Equivalent to whitening each column of the input matrix. */ Matrix Whiten(const Matrix& H) const; + + /** + * In-place version + */ + void WhitenInPlace(Matrix& H) const; }; /** @@ -64,6 +79,24 @@ namespace gtsam { // FD: does not work, ambiguous overload :-( // inline Vector operator*(const GaussianNoiseModel& R, const Vector& v) {return R.whiten(v);} + /** + * UnitCovariance: i.i.d. noise on all m dimensions. + */ + class UnitCovariance : public GaussianNoiseModel { + protected: + double sigma_; + double invsigma_; + + UnitCovariance(size_t dim): GaussianNoiseModel(dim) {} + + public: + Vector whiten(const Vector& v) const { return v; } + Vector unwhiten(const Vector& v) const { return v; } + Matrix R() const { return eye(dim_); } + Matrix Whiten(const Matrix& H) const { return H; } + void WhitenInPlace(Matrix& H) const {} + }; + /** * An isotropic noise model corresponds to a scaled diagonal covariance * This class has no public constructors. Instead, use either either the @@ -71,18 +104,16 @@ namespace gtsam { */ class Isotropic : public GaussianNoiseModel { protected: - size_t n_; double sigma_; double invsigma_; - Isotropic(size_t n, double sigma): n_(n), sigma_(sigma), invsigma_(1.0/sigma) {} - Isotropic(const Isotropic& isotropic): - n_(isotropic.n_), sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {} + Isotropic(size_t dim, double sigma) : + GaussianNoiseModel(dim), sigma_(sigma), invsigma_(1.0 / sigma) {} public: Vector whiten(const Vector& v) const; Vector unwhiten(const Vector& v) const; - Matrix R() const { return diag(repeat(n_,invsigma_)); } + Matrix R() const { return diag(repeat(dim_,invsigma_)); } }; /** @@ -90,7 +121,6 @@ namespace gtsam { */ class Sigma : public Isotropic { public: - Sigma(const Sigma& isotropic): Isotropic(isotropic) {} Sigma(size_t n, double sigma): Isotropic(n, sigma) {} }; @@ -99,7 +129,6 @@ namespace gtsam { */ class Variance : public Isotropic { public: - Variance(const Variance& v): Isotropic(v) {} Variance(size_t n, double variance): Isotropic(n, sqrt(variance)) {} }; @@ -113,9 +142,7 @@ namespace gtsam { Vector sigmas_; Vector invsigmas_; - Diagonal() {} Diagonal(const Vector& sigmas); - Diagonal(const Diagonal& d); public: Vector whiten(const Vector& v) const; @@ -130,7 +157,6 @@ namespace gtsam { */ class Sigmas : public Diagonal { public: - Sigmas(const Sigmas& s): Diagonal(s) {} Sigmas(const Vector& sigmas): Diagonal(sigmas) {} }; @@ -140,7 +166,6 @@ namespace gtsam { */ class Variances : public Diagonal { public: - Variances(const Variances& s): Diagonal(s) {} Variances(const Vector& variances); }; diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index e330779d4..d1669b8ff 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -75,6 +75,10 @@ TEST(NoiseModel, constructors) CHECK(assert_equal(expected,m3.Whiten(H))); CHECK(assert_equal(expected,m4.Whiten(H))); CHECK(assert_equal(expected,m5.Whiten(H))); + + // can only test inplace version once :-) + m5.WhitenInPlace(H); + CHECK(assert_equal(expected,H)); } /* ************************************************************************* */