12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Testing R and Whiten

release/4.3a0
Frank Dellaert 2010-01-17 03:29:23 +00:00
parent 9ddeb569b5
commit 8967027198
4 changed files with 55 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/Makefile.am
View File

@ -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

14
cpp/NoiseModel.cpp
View File

@ -13,11 +13,15 @@ typedef ublas::matrix_column<Matrix> 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<n;j++)
column(G, j) = NoiseModel::whiten(column(H, j));
Matrix GaussianNoiseModel::Whiten(const Matrix& H) const {
size_t n = H.size2(), m = H.size1();
Matrix W = zeros(m, n);
for (int j = 0; j < n; j++) {
Vector wj = whiten(column(H, j));
for (int i = 0; i < m; i++)
W(i, j) = wj(i);
}
return W;
}
Vector Isotropic::whiten(const Vector& v) const {

28
cpp/NoiseModel.h
View File

@ -8,6 +8,8 @@
#pragma once
#include <boost/shared_ptr.hpp>
//#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<NoiseModel> */ {
/**
* 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_; }
};
}

36
cpp/testNoiseModel.cpp
View File

@ -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)));
}
/* ************************************************************************* */