From 8967027198b07fa505eb0ad9b72bc3b59305ff73 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2010 03:29:23 +0000 Subject: [PATCH] Testing R and Whiten --- cpp/Makefile.am | 4 ++-- cpp/NoiseModel.cpp | 14 +++++++++----- cpp/NoiseModel.h | 28 ++++++++++++++++++---------- cpp/testNoiseModel.cpp | 36 ++++++++++++++++++++++++++---------- 4 files changed, 55 insertions(+), 27 deletions(-) diff --git a/cpp/Makefile.am b/cpp/Makefile.am index d90a5f098..fc2c8a434 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -100,8 +100,8 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp testBinaryBayesNet_LDADD = libgtsam.la # Gaussian inference -headers += GaussianFactorSet.h NoiseModel.h -sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp NoiseModel.cpp +headers += GaussianFactorSet.h +sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel testVectorConfig_SOURCES = testVectorConfig.cpp testVectorConfig_LDADD = libgtsam.la diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index bcbe26866..3d6bcb673 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -13,11 +13,15 @@ typedef ublas::matrix_column column; namespace gtsam { - Matrix GaussianNoiseModel::whiten(const Matrix& H) { - size_t n = H.size2(), m = H.size1(); - Matrix G(m,n); - for(int j=0;j +//#include "Testable.h" TODO #include "Vector.h" #include "Matrix.h" @@ -19,7 +21,7 @@ namespace gtsam { * It must implement a 'whiten' function to normalize an error vector, and an * 'unwhiten' function to unnormalize an error vector. */ - struct NoiseModel { + struct NoiseModel /* TODO : public Testable */ { /** * Whiten an error vector. @@ -29,7 +31,7 @@ namespace gtsam { /** * Unwhiten an error vector. */ - virtual Vector unwhiten(const Vector& v) const = 0; + virtual Vector unwhiten(const Vector& v) const = 0; }; /** @@ -44,12 +46,16 @@ namespace gtsam { */ struct GaussianNoiseModel : public NoiseModel { + /** + * Return R itself, but note that Whiten(H) is cheaper than R*H + */ + virtual Matrix R() const = 0; + /** * Multiply a derivative with R (derivative of whiten) * Equivalent to whitening each column of the input matrix. */ - Matrix whiten(const Matrix& H); - + Matrix Whiten(const Matrix& H) const; }; /** @@ -65,16 +71,18 @@ namespace gtsam { */ class Isotropic : public GaussianNoiseModel { protected: + size_t n_; double sigma_; double invsigma_; - Isotropic(double sigma): sigma_(sigma), invsigma_(1.0/sigma) {} + Isotropic(size_t n, double sigma): n_(n), sigma_(sigma), invsigma_(1.0/sigma) {} Isotropic(const Isotropic& isotropic): - sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {} + n_(isotropic.n_), sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {} public: Vector whiten(const Vector& v) const; Vector unwhiten(const Vector& v) const; + Matrix R() const { return diag(repeat(n_,invsigma_)); } }; /** @@ -83,7 +91,7 @@ namespace gtsam { class Sigma : public Isotropic { public: Sigma(const Sigma& isotropic): Isotropic(isotropic) {} - Sigma(double sigma): Isotropic(sigma) {} + Sigma(size_t n, double sigma): Isotropic(n, sigma) {} }; /** @@ -92,7 +100,7 @@ namespace gtsam { class Variance : public Isotropic { public: Variance(const Variance& v): Isotropic(v) {} - Variance(double variance): Isotropic(sqrt(variance)) {} + Variance(size_t n, double variance): Isotropic(n, sqrt(variance)) {} }; /** @@ -112,6 +120,7 @@ namespace gtsam { public: Vector whiten(const Vector& v) const; Vector unwhiten(const Vector& v) const; + Matrix R() const { return diag(invsigmas_); } }; /** @@ -144,12 +153,11 @@ namespace gtsam { 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; + Matrix R() const { return sqrt_inv_covariance_; } }; } diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index 7d0447e8a..e330779d4 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -23,8 +23,8 @@ TEST(NoiseModel, constructors) Vector unwhitened = Vector_(3,10.0,20.0,30.0); // Construct noise models - Sigma m1(sigma); - Variance m2(var); + Sigma m1(3,sigma); + Variance m2(3,var); Sigmas m3(Vector_(3, sigma, sigma, sigma)); Variances m4(Vector_(3, var, var, var)); FullCovariance m5(Matrix_(3, 3, @@ -46,19 +46,35 @@ TEST(NoiseModel, constructors) CHECK(assert_equal(unwhitened,m4.unwhiten(whitened))); CHECK(assert_equal(unwhitened,m5.unwhiten(whitened))); + // test R matrix + double s_1 = 1.0/sigma; + Matrix expectedR(Matrix_(3, 3, + s_1, 0.0, 0.0, + 0.0, s_1, 0.0, + 0.0, 0.0, s_1)); + + CHECK(assert_equal(expectedR,m1.R())); + CHECK(assert_equal(expectedR,m2.R())); + CHECK(assert_equal(expectedR,m3.R())); + CHECK(assert_equal(expectedR,m4.R())); + CHECK(assert_equal(expectedR,m5.R())); + + // test Whiten operator Matrix H(Matrix_(3, 4, - 1.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, - 0.0, 0.0, 1.0, 1.0)); + 1.0, 0.0, 0.0, 1.0)); Matrix expected(Matrix_(3, 4, - 1.0, 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0, 1.0, - 0.0, 0.0, 1.0, 1.0)); - - // test operator* -// CHECK(assert_equal(expected,m2.whiten(H))); + 0.0, 0.0, s_1, s_1, + 0.0, s_1, 0.0, s_1, + s_1, 0.0, 0.0, s_1)); + CHECK(assert_equal(expected,m1.Whiten(H))); + CHECK(assert_equal(expected,m2.Whiten(H))); + CHECK(assert_equal(expected,m3.Whiten(H))); + CHECK(assert_equal(expected,m4.Whiten(H))); + CHECK(assert_equal(expected,m5.Whiten(H))); } /* ************************************************************************* */