From b47438a86ca3b0292431816daf69bdee3de4de80 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Jan 2010 00:26:49 +0000 Subject: [PATCH] Smart named constructors Covariance/Variances/Variance --- cpp/GaussianFactor.h | 11 ++++- cpp/NoiseModel.cpp | 51 +++++++++++++++++++---- cpp/NoiseModel.h | 16 ++++--- cpp/testGaussianFactor.cpp | 85 +++++++++----------------------------- cpp/testNoiseModel.cpp | 17 ++++++++ 5 files changed, 97 insertions(+), 83 deletions(-) diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 6cd77b1d2..18d27efb7 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -60,7 +60,7 @@ public: As_.insert(make_pair(key1, A1)); } - /** Construct unary factor with vector of sigmas*/ + /** Construct unary factor with vector of sigmas */ GaussianFactor(const Symbol& key1, const Matrix& A1, const Vector& b, const Vector& sigmas) : b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) { @@ -76,6 +76,15 @@ public: As_.insert(make_pair(key2, A2)); } + /** Construct binary factor with vector of sigmas */ + GaussianFactor(const Symbol& key1, const Matrix& A1, + const Symbol& key2, const Matrix& A2, + const Vector& b, const Vector& sigmas) : + b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) { + As_.insert(make_pair(key1, A1)); + As_.insert(make_pair(key2, A2)); + } + /** Construct ternary factor */ GaussianFactor(const Symbol& key1, const Matrix& A1, const Symbol& key2, const Matrix& A2, diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index 8745fbd9f..bd50756c9 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -8,6 +8,8 @@ #include #include +#include +#include #include #include #include @@ -23,13 +25,30 @@ namespace gtsam { namespace noiseModel { /* ************************************************************************* */ - void Gaussian::print(const string& name) const { + Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { + size_t m = covariance.size1(), n = covariance.size2(); + if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); + if (smart) { + // check all non-diagonal entries + int i,j; + for (i = 0; i < m; i++) + for (j = 0; j < n; j++) + if (i != j && fabs(covariance(i, j) > 1e-9)) goto full; + Vector variances(n); + for (j = 0; j < n; j++) variances(j) = covariance(j,j); + return Diagonal::Variances(variances,true); + } + full: return shared_ptr(new Gaussian(inverse_square_root(covariance))); + } + + void Gaussian::print(const string& name) const { gtsam::print(sqrt_information_, "Gaussian"); } - bool Gaussian::equals(const Base& m, double tol) const { - const Gaussian* p = dynamic_cast (&m); + bool Gaussian::equals(const Base& expected, double tol) const { + const Gaussian* p = dynamic_cast (&expected); if (p == NULL) return false; + if (typeid(*this) != typeid(*p)) return false; return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol)); } @@ -76,11 +95,22 @@ namespace gtsam { /* ************************************************************************* */ // TODO: can we avoid calling reciprocal twice ? Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)), - sigmas_(sigmas) { - } + Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)), + sigmas_(sigmas) { + } - void Diagonal::print(const string& name) const { + Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { + if (smart) { + // check whether all the same entry + int j, n = variances.size(); + for (j = 1; j < n; j++) + if (variances(j) != variances(0)) goto full; + return Isotropic::Variance(n, variances(0), true); + } + full: return shared_ptr(new Diagonal(esqrt(variances))); + } + + void Diagonal::print(const string& name) const { gtsam::print(sigmas_, "Diagonal sigmas " + name); } @@ -205,7 +235,12 @@ namespace gtsam { /* ************************************************************************* */ - void Isotropic::print(const string& name) const { + Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) { + if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim); + return shared_ptr(new Isotropic(dim, sqrt(variance))); + } + + void Isotropic::print(const string& name) const { cout << "Isotropic sigma " << name << " " << sigma_ << endl; } diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h index 16490c025..0b02371ae 100644 --- a/cpp/NoiseModel.h +++ b/cpp/NoiseModel.h @@ -85,6 +85,7 @@ namespace gtsam { namespace noiseModel { /** * 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)); @@ -92,10 +93,9 @@ namespace gtsam { namespace noiseModel { /** * 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) { - return shared_ptr(new Gaussian(inverse_square_root(covariance))); - } + static shared_ptr Covariance(const Matrix& covariance, bool smart=false); /** * A Gaussian noise model created by specifying an information matrix. @@ -183,10 +183,9 @@ namespace gtsam { namespace noiseModel { /** * 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) { - return shared_ptr(new Diagonal(esqrt(variances))); - } + static shared_ptr Variances(const Vector& variances, bool smart = false); /** * A diagonal noise model created by specifying a Vector of precisions, i.e. @@ -288,10 +287,9 @@ namespace gtsam { namespace noiseModel { /** * 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) { - return shared_ptr(new Isotropic(dim, sqrt(variance))); - } + static shared_ptr Variance(size_t dim, double variance, bool smart = false); /** * An isotropic noise model created by specifying a precision diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index 666a53dda..b284c4da6 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -221,36 +221,16 @@ TEST( NonlinearFactorGraph, combine2){ /* ************************************************************************* */ TEST( GaussianFactor, linearFactorN){ + Matrix I = eye(2); vector f; - f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2, - 1.0, 0.0, - 0.0, 1.0), - Vector_(2, - 10.0, 5.0), 1))); - f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2, - -10.0, 0.0, - 0.0, -10.0), - "x2", Matrix_(2,2, - 10.0, 0.0, - 0.0, 10.0), - Vector_(2, - 1.0, -2.0), 1))); - f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", Matrix_(2,2, - -10.0, 0.0, - 0.0, -10.0), - "x3", Matrix_(2,2, - 10.0, 0.0, - 0.0, 10.0), - Vector_(2, - 1.5, -1.5), 1))); - f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", Matrix_(2,2, - -10.0, 0.0, - 0.0, -10.0), - "x4", Matrix_(2,2, - 10.0, 0.0, - 0.0, 10.0), - Vector_(2, - 2.0, -1.0), 1))); + f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2, + 10.0, 5.0), 1))); + f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I, + "x2", 10 * I, Vector_(2, 1.0, -2.0), 1))); + f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", -10 * I, + "x3", 10 * I, Vector_(2, 1.5, -1.5), 1))); + f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", -10 * I, + "x4", 10 * I, Vector_(2, 2.0, -1.0), 1))); GaussianFactor combinedFactor(f); @@ -294,8 +274,9 @@ TEST( GaussianFactor, linearFactorN){ Vector b = Vector_(8, 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); - GaussianFactor expected(combinedMeasurement, b, 1.); - CHECK(combinedFactor.equals(expected)); + Vector sigmas = repeat(8,1.0); + GaussianFactor expected(combinedMeasurement, b, sigmas); + CHECK(assert_equal(expected,combinedFactor)); } /* ************************************************************************* */ @@ -336,45 +317,19 @@ TEST( GaussianFactor, eliminate ) boost::tie(actualCG,actualLF) = combined.eliminate("x2"); // create expected Conditional Gaussian - Matrix R11 = Matrix_(2,2, - 1.0, 0.0, - 0.0, 1.0 - ); - Matrix S12 = Matrix_(2,2, - -0.2, 0.0, - +0.0,-0.2 - ); - Matrix S13 = Matrix_(2,2, - -0.8, 0.0, - +0.0,-0.8 - ); + Matrix I = eye(2); + Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I; Vector d(2); d(0) = 0.2; d(1) = -0.14; - Vector sigmas(2); - sigmas(0) = 1/sqrt(125.0); - sigmas(1) = 1/sqrt(125.0); - // Check the conditional Gaussian - GaussianConditional expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas); + GaussianConditional + expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1 / sqrt(125.0))); // the expected linear factor - double sigma = 0.2236; - Matrix Bl1 = Matrix_(2,2, - // l1 - 1.00, 0.00, - 0.00, 1.00 - ); - - Matrix Bx1 = Matrix_(2,2, - // x1 - -1.00, 0.00, - +0.00, -1.00 - ); - - // the RHS + Matrix Bl1 = I, Bx1 = -I; Vector b1(2); b1(0) = 0.0; b1(1) = 0.2; - GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma); + GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,0.2236)); // check if the result matches CHECK(assert_equal(expectedCG,*actualCG,1e-4)); @@ -452,7 +407,7 @@ TEST( GaussianFactor, eliminate2 ) // the RHS Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; - GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, sigma); + GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, repeat(2,sigma)); // check if the result matches CHECK(assert_equal(expectedCG,*actualCG,1e-4)); @@ -684,7 +639,7 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l11",S12,sigmas)); GaussianFactor actualLF(CG); // actualLF.print(); - GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas(0)); + GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas); CHECK(assert_equal(expectedLF,actualLF,1e-5)); } diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index f4a8ca76f..9fa04ee6f 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -173,6 +173,23 @@ TEST( NoiseModel, QR ) CHECK(assert_equal(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! } +/* ************************************************************************* */ +TEST(NoiseModel, SmartCovariance ) +{ + bool smart = true; + sharedGaussian expected = Unit::Create(3); + sharedGaussian actual = Gaussian::Covariance(eye(3), smart); + CHECK(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, ScalarOrVector ) +{ + sharedGaussian expected = Unit::Create(3); + sharedGaussian actual = Gaussian::Covariance(eye(3), smart); + CHECK(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ int main() { TestResult tr;