From ea149598356398db948743ddb7d481305459bdd8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2010 17:47:23 +0000 Subject: [PATCH] NoiseModel inhertance structure reversed, all constructors protected, new static "factories" --- cpp/Matrix.cpp | 5 +- cpp/NoiseModel.cpp | 50 ++++----- cpp/NoiseModel.h | 236 ++++++++++++++++++++++++----------------- cpp/PriorFactor.h | 2 +- cpp/Vector.cpp | 7 ++ cpp/Vector.h | 7 ++ cpp/testMatrix.cpp | 131 ++++++++++++++++++++--- cpp/testNoiseModel.cpp | 59 +++++------ cpp/testPose2Prior.cpp | 2 +- myconfigure | 2 +- 10 files changed, 327 insertions(+), 174 deletions(-) diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 759abb943..114ddd3e2 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -300,7 +300,8 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) { // update A, b // A' \define A_{S}-ar and b'\define b-ad // __attribute__ ((noinline)) // uncomment to prevent inlining when profiling -void updateAb(Matrix& A, Vector& b, int j, const Vector& a, const Vector& r, double d) { +static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, + const Vector& r, double d) { const size_t m = A.size1(), n = A.size2(); for (int i = 0; i < m; i++) { // update all rows double ai = a(i); @@ -308,7 +309,7 @@ void updateAb(Matrix& A, Vector& b, int j, const Vector& a, const Vector& r, dou double *Aij = A.data().begin() + i * n + j + 1; const double *rptr = r.data().begin() + j + 1; // A(i,j+1:end) -= ai*r(j+1:end) - for (int j2 = j + 1; j2 < n; j2++,Aij++,rptr++) + for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++) *Aij -= ai * (*rptr); } } diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index eb7637246..b506aa282 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -13,6 +13,15 @@ typedef ublas::matrix_column column; namespace gtsam { + /* ************************************************************************* */ + Vector GaussianNoiseModel::whiten(const Vector& v) const { + return sqrt_information_ * v; + } + + Vector GaussianNoiseModel::unwhiten(const Vector& v) const { + return backSubstituteUpper(sqrt_information_, v); + } + // functional Matrix GaussianNoiseModel::Whiten(const Matrix& H) const { size_t m = H.size1(), n = H.size2(); @@ -35,17 +44,11 @@ namespace gtsam { } } - Vector Isotropic::whiten(const Vector& v) const { - return v * invsigma_; - } - - Vector Isotropic::unwhiten(const Vector& v) const { - return v * sigma_; - } - + /* ************************************************************************* */ + // TODO: can we avoid calling reciprocal twice ? Diagonal::Diagonal(const Vector& sigmas) : - GaussianNoiseModel(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal( - sigmas)) { + GaussianNoiseModel(diag(reciprocal(sigmas))), + invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) { } Vector Diagonal::whiten(const Vector& v) const { @@ -56,28 +59,15 @@ namespace gtsam { return emul(v, sigmas_); } - static Vector sqrt(const Vector& v) { - Vector s(v.size()); - transform(v.begin(), v.end(), s.begin(), ::sqrt); - return s; + /* ************************************************************************* */ + + Vector Isotropic::whiten(const Vector& v) const { + return v * invsigma_; } - Variances::Variances(const Vector& variances) : - Diagonal(sqrt(variances)) { - } - - FullCovariance::FullCovariance(const Matrix& cov) : - GaussianNoiseModel(cov.size1()), - sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_( - inverse_square_root(cov)) { - } - - Vector FullCovariance::whiten(const Vector& v) const { - return sqrt_inv_covariance_ * v; - } - - Vector FullCovariance::unwhiten(const Vector& v) const { - return sqrt_covariance_ * v; + Vector Isotropic::unwhiten(const Vector& v) const { + return v * sigma_; } +/* ************************************************************************* */ } // gtsam diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h index 5658a4619..36be79eba 100644 --- a/cpp/NoiseModel.h +++ b/cpp/NoiseModel.h @@ -30,6 +30,7 @@ namespace gtsam { public: NoiseModel(size_t dim):dim_(dim) {} + virtual ~NoiseModel() {} /** * Whiten an error vector. @@ -39,7 +40,7 @@ namespace gtsam { /** * Unwhiten an error vector. */ - virtual Vector unwhiten(const Vector& v) const = 0; + virtual Vector unwhiten(const Vector& v) const = 0; }; /** @@ -50,139 +51,180 @@ namespace gtsam { * x = unwhiten(x) = inv(R)*y * as indeed * |y|^2 = y'*y = x'*R'*R*x - * Various simplified models are available that implement this efficiently. + * Various derived classes are available that are more efficient. */ - struct GaussianNoiseModel : public NoiseModel { + struct GaussianNoiseModel: public NoiseModel { - GaussianNoiseModel(size_t dim):NoiseModel(dim) {} + protected: + + // TODO: store as boost upper-triangular or whatever is passed from above + /* Matrix square root of information matrix (R) */ + Matrix sqrt_information_; + + /** protected constructor takes square root information matrix */ + GaussianNoiseModel(const Matrix& sqrt_information) : + NoiseModel(sqrt_information.size1()), sqrt_information_(sqrt_information) { + } + + public: + + typedef boost::shared_ptr shared_ptr; /** - * Return R itself, but note that Whiten(H) is cheaper than R*H - */ - virtual Matrix R() const = 0; + * A Gaussian noise model created by specifying a square root information matrix. + */ + static shared_ptr SqrtInformation(const Matrix& R) { + return shared_ptr(new GaussianNoiseModel(R)); + } - /** - * Multiply a derivative with R (derivative of whiten) - * Equivalent to whitening each column of the input matrix. - */ - Matrix Whiten(const Matrix& H) const; + /** + * A Gaussian noise model created by specifying a covariance matrix. + */ + static shared_ptr Covariance(const Matrix& Sigma) { + return shared_ptr(new GaussianNoiseModel(inverse_square_root(Sigma))); + } - /** - * In-place version - */ - void WhitenInPlace(Matrix& H) const; - }; + /** + * A Gaussian noise model created by specifying an information matrix. + */ + static shared_ptr Information(const Matrix& Q) { + return shared_ptr(new GaussianNoiseModel(square_root_positive(Q))); + } + + virtual Vector whiten(const Vector& v) const; + virtual Vector unwhiten(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; + + /** + * Return R itself, but note that Whiten(H) is cheaper than R*H + */ + const Matrix& R() const { + return sqrt_information_; + } + + }; // GaussianNoiseModel - /** - * We identify the Gaussian noise model with R - */ // 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. + * 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 UnitCovariance : public GaussianNoiseModel { + class Diagonal : public GaussianNoiseModel { protected: - double sigma_; - double invsigma_; - UnitCovariance(size_t dim): GaussianNoiseModel(dim) {} + /** sigmas and reciprocal */ + Vector sigmas_, invsigmas_; + + /** protected constructor takes sigmas */ + Diagonal(const Vector& sigmas); 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 {} + + 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) { + return shared_ptr(new Diagonal(sigmas)); + } + + /** + * A diagonal noise model created by specifying a Vector of variances, i.e. + * i.e. the diagonal of the covariance matrix. + */ + static shared_ptr Variances(const Vector& variances) { + return shared_ptr(new Diagonal(esqrt(variances))); + } + + /** + * 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 Vector whiten(const Vector& v) const; + virtual Vector unwhiten(const Vector& v) const; }; /** * An isotropic noise model corresponds to a scaled diagonal covariance - * This class has no public constructors. Instead, use either either the - * Sigma or Variance class. + * To construct, use one of the static methods */ - class Isotropic : public GaussianNoiseModel { + class Isotropic : public Diagonal { protected: - double sigma_; - double invsigma_; + double sigma_, invsigma_; + /** protected constructor takes sigma */ Isotropic(size_t dim, double sigma) : - GaussianNoiseModel(dim), sigma_(sigma), invsigma_(1.0 / sigma) {} + Diagonal(repeat(dim, sigma)),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(dim_,invsigma_)); } + + 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. + */ + static shared_ptr Variance(size_t dim, double variance) { + return shared_ptr(new Isotropic(dim, sqrt(variance))); + } + + /** + * 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 Vector whiten(const Vector& v) const; + virtual Vector unwhiten(const Vector& v) const; }; /** - * An isotropic noise model using sigma, the standard deviation. + * UnitCovariance: i.i.d. unit-variance noise on all m dimensions. */ - class Sigma : public Isotropic { - public: - Sigma(size_t n, double sigma): Isotropic(n, sigma) {} - }; - - /** - * An isotropic noise model using the noise variance = sigma^2. - */ - class Variance : public Isotropic { - public: - Variance(size_t n, double variance): Isotropic(n, sqrt(variance)) {} - }; - - /** - * 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 either the Sigmas or Variances class. - */ - class Diagonal : public GaussianNoiseModel { + class UnitCovariance : public Isotropic { protected: - Vector sigmas_; - Vector invsigmas_; - Diagonal(const Vector& sigmas); + UnitCovariance(size_t dim): Isotropic(dim,1.0) {} public: - Vector whiten(const Vector& v) const; - Vector unwhiten(const Vector& v) const; - Matrix R() const { return diag(invsigmas_); } - }; - /** - * A diagonal noise model created by specifying a Vector of sigmas, i.e. - * standard devations, i.e. the diagonal of the square root covariance - * matrix. - */ - class Sigmas : public Diagonal { - public: - Sigmas(const Vector& sigmas): Diagonal(sigmas) {} - }; + typedef boost::shared_ptr shared_ptr; - /** - * A diagonal noise model created by specifying a Vector of variances, i.e. - * i.e. the diagonal of the covariance matrix. - */ - class Variances : public Diagonal { - public: - Variances(const Vector& variances); - }; + /** + * An isotropic noise model created by specifying a standard devation sigma + */ + static shared_ptr Create(size_t dim); - /** - * A full covariance noise model. - */ - class FullCovariance : public GaussianNoiseModel { - protected: - Matrix sqrt_covariance_; - 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_; } + 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 {} }; } diff --git a/cpp/PriorFactor.h b/cpp/PriorFactor.h index e968f2d71..d3d21884b 100644 --- a/cpp/PriorFactor.h +++ b/cpp/PriorFactor.h @@ -38,7 +38,7 @@ namespace gtsam { /** Constructor */ PriorFactor(const Key& key, const T& prior, const Matrix& cov) : - Base(1.0, key), prior_(prior), noiseModel_(new FullCovariance(cov)) { + Base(1.0, key), prior_(prior), noiseModel_(GaussianNoiseModel::Covariance(cov)) { } /** implement functions needed for Testable */ diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp index 475537878..914f76e61 100644 --- a/cpp/Vector.cpp +++ b/cpp/Vector.cpp @@ -204,6 +204,13 @@ namespace gtsam { return b; } + /* ************************************************************************* */ + Vector esqrt(const Vector& v) { + Vector s(v.size()); + transform(v.begin(), v.end(), s.begin(), ::sqrt); + return s; + } + /* ************************************************************************* */ double max(const Vector &a) { return *(std::max_element(a.begin(), a.end())); diff --git a/cpp/Vector.h b/cpp/Vector.h index 2a028ce15..02f5930e8 100644 --- a/cpp/Vector.h +++ b/cpp/Vector.h @@ -177,6 +177,13 @@ double sum(const Vector &a); */ Vector reciprocal(const Vector &a); +/** + * elementwise sqrt of vector elements + * @param a vector + * @return [sqrt(a(i))] + */ +Vector esqrt(const Vector& v); + /** * return the max element of a vector * @param a vector diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index 9357c2e7c..e1e529049 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -9,7 +9,9 @@ #include #include #include +#include #include "Matrix.h" +#include "NoiseModel.h" using namespace std; using namespace gtsam; @@ -545,6 +547,87 @@ TEST( matrix, svd ) EQUALITY(S,S1); } +/* ************************************************************************* */ +// update A, b +// A' \define A_{S}-ar and b'\define b-ad +// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling +static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, + const Vector& r, double d) { + const size_t m = A.size1(), n = A.size2(); + for (int i = 0; i < m; i++) { // update all rows + double ai = a(i); + b(i) -= ai * d; + double *Aij = A.data().begin() + i * n + j + 1; + const double *rptr = r.data().begin() + j + 1; + // A(i,j+1:end) -= ai*r(j+1:end) + for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++) + *Aij -= ai * (*rptr); + } +} + +/* ************************************************************************* */ +list > +weighted_eliminate2(Matrix& A, Vector& b, const GaussianNoiseModel& model) { + size_t m = A.size1(), n = A.size2(); // get size(A) + size_t maxRank = min(m,n); + + // pre-whiten everything + model.WhitenInPlace(A); + b = model.whiten(b); + + // create list + list > results; + + // We loop over all columns, because the columns that can be eliminated + // are not necessarily contiguous. For each one, estimate the corresponding + // scalar variable x as d-rS, with S the separator (remaining columns). + // Then update A and b by substituting x with d-rS, zero-ing out x's column. + for (int j=0; j(A, j2)); + + // create the rhs + double d = inner_prod(pseudo, b); + + // construct solution (r, d, sigma) + // TODO: avoid sqrt, store precision or at least variance + results.push_back(boost::make_tuple(r, d, 1./sqrt(precision))); + + // exit after rank exhausted + if (results.size()>=maxRank) break; + + // update A, b, expensive, suing outer product + // A' \define A_{S}-a*r and b'\define b-d*a + updateAb(A, b, j, a, r, d); + } + + return results; +} + +/* ************************************************************************* */ +void weighted_eliminate3(Matrix& A, Vector& b, const GaussianNoiseModel& model) { + size_t m = A.size1(), n = A.size2(); // get size(A) + size_t maxRank = min(m,n); + + // pre-whiten everything + model.WhitenInPlace(A); + b = model.whiten(b); + + householder_(A, maxRank); +} + /* ************************************************************************* */ TEST( matrix, weighted_elimination ) { @@ -552,21 +635,17 @@ TEST( matrix, weighted_elimination ) Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., - 1., 0., 0., 0., -1., 0., - 0., 1., 0., 0., 0., -1.); + 1., 0., 0., 0., -1., 0., + 0., 1., 0., 0., 0., -1.); Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); - // perform elimination - std::list > solution = - weighted_eliminate(A, b, sigmas); - // expected values Matrix expectedR = Matrix_(4, 6, - 1., 0.,-0.2, 0.,-0.8, 0., - 0., 1., 0.,-0.2, 0.,-0.8, - 0., 0., 1., 0., -1., 0., - 0., 0., 0., 1., 0., -1.); + 1., 0., -0.2, 0., -0.8, 0., + 0., 1., 0.,-0.2, 0., -0.8, + 0., 0., 1., 0., -1., 0., + 0., 0., 0., 1., 0., -1.); Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); Vector newSigmas = Vector_(4, 0.0894427, @@ -574,15 +653,43 @@ TEST( matrix, weighted_elimination ) 0.223607, 0.223607); - // unpack and verify Vector r; double di, sigma; - size_t i = 0; + size_t i; + + // perform elimination + Matrix A1 = A; Vector b1 = b; + std::list > solution = + weighted_eliminate(A1, b1, sigmas); + + // unpack and verify + i=0; BOOST_FOREACH(boost::tie(r, di, sigma), solution) { CHECK(assert_equal(r, row(expectedR, i))); // verify r DOUBLES_EQUAL(d(i), di, 1e-8); // verify d DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma i += 1; } + + // perform elimination with NoiseModel + Matrix A2 = A; Vector b2 = b; + GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(sigmas); + std::list > solution2 = + weighted_eliminate2(A2, b2, *model); + + // unpack and verify + i=0; + BOOST_FOREACH(boost::tie(r, di, sigma), solution2) { + CHECK(assert_equal(r, row(expectedR, i))); // verify r + DOUBLES_EQUAL(d(i), di, 1e-8); // verify d + DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma + i += 1; + } + + // perform elimination with NoiseModel + weighted_eliminate3(A, b, *model); + GaussianNoiseModel::shared_ptr newModel = Diagonal::Sigmas(newSigmas); +// print(A); +// print(newModel->Whiten(expectedR)); } /* ************************************************************************* */ diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index d1669b8ff..d26817804 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -8,7 +8,7 @@ #include -#include +#include #include #include "NoiseModel.h" @@ -18,46 +18,48 @@ using namespace gtsam; /* ************************************************************************* */ TEST(NoiseModel, constructors) { - double sigma = 2, var = sigma*sigma; + double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; Vector whitened = Vector_(3,5.0,10.0,15.0); Vector unwhitened = Vector_(3,10.0,20.0,30.0); // Construct noise models - 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, + vector m; + m.push_back(GaussianNoiseModel::SqrtInformation(Matrix_(3, 3, + s_1, 0.0, 0.0, + 0.0, s_1, 0.0, + 0.0, 0.0, s_1))); + m.push_back(GaussianNoiseModel::Covariance(Matrix_(3, 3, var, 0.0, 0.0, 0.0, var, 0.0, - 0.0, 0.0, var)); + 0.0, 0.0, var))); + m.push_back(GaussianNoiseModel::Information(Matrix_(3, 3, + prc, 0.0, 0.0, + 0.0, prc, 0.0, + 0.0, 0.0, prc))); + m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma))); + m.push_back(Diagonal::Variances(Vector_(3, var, var, var))); + m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc))); + m.push_back(Isotropic::Sigma(3, sigma)); + m.push_back(Isotropic::Variance(3, var)); + m.push_back(Isotropic::Precision(3, prc)); // test whiten - CHECK(assert_equal(whitened,m1.whiten(unwhitened))); - CHECK(assert_equal(whitened,m2.whiten(unwhitened))); - CHECK(assert_equal(whitened,m3.whiten(unwhitened))); - CHECK(assert_equal(whitened,m4.whiten(unwhitened))); - CHECK(assert_equal(whitened,m5.whiten(unwhitened))); + int i=0; + BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + CHECK(assert_equal(whitened,mi->whiten(unwhitened))); // test unwhiten - CHECK(assert_equal(unwhitened,m1.unwhiten(whitened))); - CHECK(assert_equal(unwhitened,m2.unwhiten(whitened))); - CHECK(assert_equal(unwhitened,m3.unwhiten(whitened))); - CHECK(assert_equal(unwhitened,m4.unwhiten(whitened))); - CHECK(assert_equal(unwhitened,m5.unwhiten(whitened))); + BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + CHECK(assert_equal(unwhitened,mi->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())); + BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + CHECK(assert_equal(expectedR,mi->R())); // test Whiten operator Matrix H(Matrix_(3, 4, @@ -70,14 +72,11 @@ TEST(NoiseModel, constructors) 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))); + BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + CHECK(assert_equal(expected,mi->Whiten(H))); // can only test inplace version once :-) - m5.WhitenInPlace(H); + m[0]->WhitenInPlace(H); CHECK(assert_equal(expected,H)); } diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp index 6344b54e6..a0e9c6140 100644 --- a/cpp/testPose2Prior.cpp +++ b/cpp/testPose2Prior.cpp @@ -13,7 +13,7 @@ using namespace gtsam; // Common measurement covariance static double sx=0.5, sy=0.5,st=0.1; -static boost::shared_ptr model(new Sigmas(Vector_(3,sx,sy,st))); +static GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(Vector_(3,sx,sy,st)); /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) diff --git a/myconfigure b/myconfigure index e3433e6cc..1ebdc636b 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ ./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ # for maximum performance on Intel Core2 platform: -#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file +#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -ftree-vectorizer-verbose=2 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file