From 6f74fe49c92706534dc38d56b4598c03d20445c1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 5 Feb 2022 23:03:55 -0500 Subject: [PATCH 01/10] wrap GaussianDensity::FromMeanAndStddev --- gtsam/linear/GaussianDensity.cpp | 12 +++++++----- gtsam/linear/GaussianDensity.h | 10 +++++----- gtsam/linear/linear.i | 13 +++++++++---- 3 files changed, 21 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index d9cde9b91..2bae129be 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,9 +23,11 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma) - { - return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma); + GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, + const Vector& mean, + double sigma) { + return GaussianDensity(key, mean / sigma, + Matrix::Identity(mean.size(), mean.size()) / sigma); } /* ************************************************************************* */ @@ -35,8 +37,8 @@ namespace gtsam { for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; - gtsam::print(Matrix(R()), "R: "); - gtsam::print(Vector(d()), "d: "); + gtsam::print(mean(), "mean: "); + gtsam::print(covariance(), "covariance: "); if(model_) model_->print("Noise model: "); } diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 71af704ab..f078d5db6 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -24,11 +24,10 @@ namespace gtsam { /** - * A Gaussian density. - * - * It is implemented as a GaussianConditional without parents. + * A GaussianDensity is a GaussianConditional without parents. * The negative log-probability is given by \f$ |Rx - d|^2 \f$ * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$ + * @addtogroup linear */ class GTSAM_EXPORT GaussianDensity : public GaussianConditional { @@ -52,8 +51,9 @@ namespace gtsam { GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) : GaussianConditional(key, d, R, noiseModel) {} - /// Construct using a mean and variance - static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma); + /// Construct using a mean and standard deviation + static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, + double sigma); /// print void print(const std::string& = "GaussianDensity", diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 674287b87..52beb11f6 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -490,14 +490,19 @@ virtual class GaussianConditional : gtsam::JacobianFactor { #include virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + // Constructors + GaussianDensity(gtsam::Key key, Vector d, Matrix R, + const gtsam::noiseModel::Diagonal* sigmas); - //Standard Interface + static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key, + const Vector& mean, + double sigma); + + // Standard Interface void print(string s = "GaussianDensity", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; + bool equals(const gtsam::GaussianDensity& cg, double tol) const; Vector mean() const; Matrix covariance() const; }; From 476a2539f4985768eae9f9cfef6eb85e40bdb6f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 12:53:27 -0500 Subject: [PATCH 02/10] Working with sigma in matrices --- gtsam/linear/GaussianConditional.cpp | 48 ++++++++++++++----- gtsam/linear/GaussianConditional.h | 25 +++++++--- gtsam/linear/linear.i | 14 ++++++ .../linear/tests/testGaussianConditional.cpp | 31 +++++++++++- 4 files changed, 99 insertions(+), 19 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index d87c39eea..6a6f75d73 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -43,19 +43,45 @@ namespace gtsam { Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) : BaseFactor(key, R, d, sigmas), BaseConditional(1) {} - /* ************************************************************************* */ - GaussianConditional::GaussianConditional( - Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, const SharedDiagonal& sigmas) : - BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {} + /* ************************************************************************ */ + GaussianConditional::GaussianConditional(Key key, const Vector& d, + const Matrix& R, Key parent1, + const Matrix& S, + const SharedDiagonal& sigmas) + : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {} - /* ************************************************************************* */ - GaussianConditional::GaussianConditional( - Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) : - BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} + /* ************************************************************************ */ + GaussianConditional::GaussianConditional(Key key, const Vector& d, + const Matrix& R, Key parent1, + const Matrix& S, Key parent2, + const Matrix& T, + const SharedDiagonal& sigmas) + : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas), + BaseConditional(1) {} - /* ************************************************************************* */ + /* ************************************************************************ */ + GaussianConditional GaussianConditional::FromMeanAndStddev( + Key key, const Matrix& A, Key parent, const Vector& b, double sigma) { + // |Rx + Sy - d| = |x-(Ay + b)|/sigma + const Matrix R = Matrix::Identity(b.size(), b.size()) / sigma; + const Matrix S = -A / sigma; + const Vector d = b / sigma; + return GaussianConditional(key, d, R, parent, S); + } + + /* ************************************************************************ */ + GaussianConditional GaussianConditional::FromMeanAndStddev( + Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2, + const Vector& b, double sigma) { + // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma + const Matrix R = Matrix::Identity(b.size(), b.size()) / sigma; + const Matrix S = -A1 / sigma; + const Matrix T = -A2 / sigma; + const Vector d = b / sigma; + return GaussianConditional(key, d, R, parent1, S, parent2, T); + } + + /* ************************************************************************ */ void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " Conditional density "; for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d93f65b42..12d85d98f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -29,9 +29,10 @@ namespace gtsam { /** - * A conditional Gaussian functions as the node in a Bayes network + * A GaussianConditional functions as the node in a Bayes network. * It has a set of parents y,z, etc. and implements a probability density on x. * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ + * @addtogroup linear */ class GTSAM_EXPORT GaussianConditional : public JacobianFactor, @@ -51,13 +52,14 @@ namespace gtsam { const SharedDiagonal& sigmas = SharedDiagonal()); /** constructor with only one parent |Rx+Sy-d| */ - GaussianConditional(Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal()); + GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1, + const Matrix& S, + const SharedDiagonal& sigmas = SharedDiagonal()); /** constructor with two parents |Rx+Sy+Tz-d| */ - GaussianConditional(Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, Key name2, const Matrix& T, - const SharedDiagonal& sigmas = SharedDiagonal()); + GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1, + const Matrix& S, Key parent2, const Matrix& T, + const SharedDiagonal& sigmas = SharedDiagonal()); /** Constructor with arbitrary number of frontals and parents. * @tparam TERMS A container whose value type is std::pair, specifying the @@ -76,6 +78,17 @@ namespace gtsam { const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /// Construct from mean A1 p1 + b and standard deviation. + static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A, + Key parent, const Vector& b, + double sigma); + + /// Construct from mean A1 p1 + A2 p2 + b and standard deviation. + static GaussianConditional FromMeanAndStddev(Key key, // + const Matrix& A1, Key parent1, + const Matrix& A2, Key parent2, + const Vector& b, double sigma); + /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by * \c first and \c last must be in increasing order, meaning that the parents of any * conditional may not include a conditional coming before it. diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 52beb11f6..46c5415aa 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -468,6 +468,20 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T); + // Named constructors + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, + const Matrix& A, + gtsam::Key parent, + const Vector& b, + double sigma); + + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, + const Matrix& A1, + gtsam::Key parent1, + const Matrix& A2, + gtsam::Key parent2, + const Vector& b, + double sigma); // Standard Interface void print(string s = "GaussianConditional", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index fae00e1e4..4483066b4 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -38,6 +38,7 @@ using namespace gtsam; using namespace std; using namespace boost::assign; +using symbol_shorthand::X; static const double tol = 1e-5; @@ -316,5 +317,31 @@ TEST( GaussianConditional, isGaussianFactor ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +// Test FromMeanAndStddev named constructors +TEST(GaussianConditional, FromMeanAndStddev) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished(); + const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6); + const double sigma = 3; + + VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2); + + auto conditional1 = + GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); + Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma; + double expected1 = 0.5 * e1.dot(e1); + EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9); + + auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2, + X(2), b, sigma); + Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma; + double expected2 = 0.5 * e2.dot(e2); + EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From bc233fc967771b00342e6b21489089687c40bfd6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 13:57:52 -0500 Subject: [PATCH 03/10] Use noiseModel for named constructors --- gtsam/linear/GaussianConditional.cpp | 20 +++++++++++--------- gtsam/linear/GaussianDensity.cpp | 5 +++-- gtsam/linear/tests/testGaussianDensity.cpp | 19 +++++++++++++++++++ 3 files changed, 33 insertions(+), 11 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6a6f75d73..21787ba7d 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -63,10 +63,11 @@ namespace gtsam { GaussianConditional GaussianConditional::FromMeanAndStddev( Key key, const Matrix& A, Key parent, const Vector& b, double sigma) { // |Rx + Sy - d| = |x-(Ay + b)|/sigma - const Matrix R = Matrix::Identity(b.size(), b.size()) / sigma; - const Matrix S = -A / sigma; - const Vector d = b / sigma; - return GaussianConditional(key, d, R, parent, S); + const Matrix R = Matrix::Identity(b.size(), b.size()); + const Matrix S = -A; + const Vector d = b; + return GaussianConditional(key, d, R, parent, S, + noiseModel::Isotropic::Sigma(b.size(), sigma)); } /* ************************************************************************ */ @@ -74,11 +75,12 @@ namespace gtsam { Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2, const Vector& b, double sigma) { // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma - const Matrix R = Matrix::Identity(b.size(), b.size()) / sigma; - const Matrix S = -A1 / sigma; - const Matrix T = -A2 / sigma; - const Vector d = b / sigma; - return GaussianConditional(key, d, R, parent1, S, parent2, T); + const Matrix R = Matrix::Identity(b.size(), b.size()); + const Matrix S = -A1; + const Matrix T = -A2; + const Vector d = b; + return GaussianConditional(key, d, R, parent1, S, parent2, T, + noiseModel::Isotropic::Sigma(b.size(), sigma)); } /* ************************************************************************ */ diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 2bae129be..2f7aa312b 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -26,8 +26,9 @@ namespace gtsam { GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, double sigma) { - return GaussianDensity(key, mean / sigma, - Matrix::Identity(mean.size(), mean.size()) / sigma); + return GaussianDensity(key, mean, + Matrix::Identity(mean.size(), mean.size()), + noiseModel::Isotropic::Sigma(mean.size(), sigma)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 29dc49591..14608e794 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -17,10 +17,13 @@ **/ #include +#include + #include using namespace gtsam; using namespace std; +using symbol_shorthand::X; /* ************************************************************************* */ TEST(GaussianDensity, constructor) @@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor) EXPECT(assert_equal(s, copied.get_model()->sigmas())); } +/* ************************************************************************* */ +// Test FromMeanAndStddev named constructor +TEST(GaussianDensity, FromMeanAndStddev) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 b(20, 40), x0(1, 2); + const double sigma = 3; + + VectorValues values; + values.insert(X(0), x0); + + auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma); + Vector2 e = (x0 - b) / sigma; + double expected = 0.5 * e.dot(e); + EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From f9e6282a2c56e97c1a09779126e3a354c71fce10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 17:31:13 -0500 Subject: [PATCH 04/10] Sampling from GaussianConditional --- gtsam/linear/GaussianConditional.cpp | 31 ++++++++++++++++++- gtsam/linear/GaussianConditional.h | 26 ++++++++++++++++ .../linear/tests/testGaussianConditional.cpp | 23 ++++++++++++++ 3 files changed, 79 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 21787ba7d..3aaffe6dc 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #ifdef __GNUC__ @@ -220,7 +221,35 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* ************************************************************************ */ + VectorValues GaussianConditional::sample( + const VectorValues& parentsValues) const { + if (nrFrontals() != 1) { + throw std::invalid_argument( + "GaussianConditional::sample can only be called on single variable " + "conditionals"); + } + if (!model_) { + throw std::invalid_argument( + "GaussianConditional::sample can only be called if a diagonal noise " + "model was specified at construction."); + } + VectorValues solution = solve(parentsValues); + Sampler sampler(model_); + Key key = firstFrontalKey(); + solution[key] += sampler.sample(); + return solution; + } + + VectorValues GaussianConditional::sample() const { + if (nrParents() != 0) + throw std::invalid_argument( + "sample() can only be invoked on no-parent prior"); + VectorValues values; + return sample(values); + } + + /* ************************************************************************ */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 void GTSAM_DEPRECATED GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 12d85d98f..e44da195b 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -44,6 +44,9 @@ namespace gtsam { typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class + /// @name Constructors + /// @{ + /** default constructor needed for serialization */ GaussianConditional() {} @@ -99,6 +102,10 @@ namespace gtsam { template static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /// @} + /// @name Testable + /// @{ + /** print */ void print(const std::string& = "GaussianConditional", const KeyFormatter& formatter = DefaultKeyFormatter) const override; @@ -106,6 +113,10 @@ namespace gtsam { /** equals function */ bool equals(const GaussianFactor&cg, double tol = 1e-9) const override; + /// @} + /// @name Standard Interface + /// @{ + /** Return a view of the upper-triangular R block of the conditional */ constABlock R() const { return Ab_.range(0, nrFrontals()); } @@ -138,10 +149,25 @@ namespace gtsam { /** Performs transpose backsubstition in place on values */ void solveTransposeInPlace(VectorValues& gy) const; + /** + * sample + * @param parentsValues Known values of the parents + * @return sample from conditional + */ + VectorValues sample(const VectorValues& parentsValues) const; + + /// Zero parent version. + VectorValues sample() const; + + /// @} + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; + /// @} #endif private: diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 4483066b4..ae9a2d94b 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -339,6 +340,28 @@ TEST(GaussianConditional, FromMeanAndStddev) { EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9); } +/* ************************************************************************* */ +// Test sampling +TEST(GaussianConditional, sample) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 b(20, 40), x1(3, 4); + const double sigma = 0.01; + + auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma); + auto actual1 = density.sample(); + EXPECT_LONGS_EQUAL(1, actual1.size()); + EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma)); + + VectorValues values; + values.insert(X(1), x1); + + auto conditional = + GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); + auto actual2 = conditional.sample(values); + EXPECT_LONGS_EQUAL(1, actual2.size()); + EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 539bf708294f26896edd0cfe45d35f6e12964271 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 17:56:49 -0500 Subject: [PATCH 05/10] Sampling from GaussianBayesNet --- gtsam/linear/GaussianBayesNet.cpp | 42 +++++++++++++-------- gtsam/linear/GaussianBayesNet.h | 13 +++++-- gtsam/linear/tests/testGaussianBayesNet.cpp | 19 ++++++++++ 3 files changed, 56 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8fd4f2c26..87b1c6cb4 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -37,28 +37,40 @@ namespace gtsam { return Base::equals(bn, tol); } - /* ************************************************************************* */ - VectorValues GaussianBayesNet::optimize() const - { - VectorValues soln; // no missing variables -> just create an empty vector - return optimize(soln); + /* ************************************************************************ */ + VectorValues GaussianBayesNet::optimize() const { + VectorValues solution; // no missing variables -> create an empty vector + return optimize(solution); } - /* ************************************************************************* */ - VectorValues GaussianBayesNet::optimize( - const VectorValues& solutionForMissing) const { - VectorValues soln(solutionForMissing); // possibly empty + VectorValues GaussianBayesNet::optimize(VectorValues solution) const { // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) - /** solve each node in turn in topological sort order (parents first)*/ - for (auto cg: boost::adaptors::reverse(*this)) { + // solve each node in reverse topological sort order (parents first) + for (auto cg : boost::adaptors::reverse(*this)) { // i^th part of R*x=y, x=inv(R)*y - // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) - soln.insert(cg->solve(soln)); + // (Rii*xi + R_i*x(i+1:))./si = yi => + // xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) + solution.insert(cg->solve(solution)); } - return soln; + return solution; } - /* ************************************************************************* */ + /* ************************************************************************ */ + VectorValues GaussianBayesNet::sample() const { + VectorValues result; // no missing variables -> create an empty vector + return sample(result); + } + + VectorValues GaussianBayesNet::sample(VectorValues result) const { + // sample each node in reverse topological sort order (parents first) + for (auto cg : boost::adaptors::reverse(*this)) { + const VectorValues sampled = cg->sample(result); + result.insert(sampled); + } + return result; + } + + /* ************************************************************************ */ VectorValues GaussianBayesNet::optimizeGradientSearch() const { gttic(GaussianBayesTree_optimizeGradientSearch); diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 6d906d65e..18da3ed8a 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -88,11 +88,18 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution + /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by + /// back-substitution VectorValues optimize() const; - /// Version of optimize for incomplete BayesNet, needs solution for missing variables - VectorValues optimize(const VectorValues& solutionForMissing) const; + /// Version of optimize for incomplete BayesNet, given missing variables + VectorValues optimize(const VectorValues given) const; + + /// Sample using ancestral sampling + VectorValues sample() const; + + /// Sample from an incomplete BayesNet, given missing variables + VectorValues sample(VectorValues given) const; /** * Return ordering corresponding to a topological sort. diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index f62da15dd..d9c6ee93d 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -16,10 +16,12 @@ */ #include +#include #include #include #include #include +#include #include #include @@ -35,6 +37,7 @@ using namespace boost::assign; using namespace std::placeholders; using namespace std; using namespace gtsam; +using symbol_shorthand::X; static const Key _x_ = 11, _y_ = 22, _z_ = 33; @@ -138,6 +141,22 @@ TEST( GaussianBayesNet, optimize3 ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianBayesNet, sample) { + GaussianBayesNet gbn; + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + const Vector2 mean(20, 40), b(10, 10); + const double sigma = 0.01; + + gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma)); + gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma)); + + auto actual = gbn.sample(); + EXPECT_LONGS_EQUAL(2, actual.size()); + EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma)); + EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma)); +} + /* ************************************************************************* */ TEST(GaussianBayesNet, ordering) { From a25378abe73f0e73c783bd9c05ebd12c95f02b9f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 20:21:04 -0500 Subject: [PATCH 06/10] wrap sampling methods --- gtsam/linear/linear.i | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 46c5415aa..113ebb8ec 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -482,15 +482,19 @@ virtual class GaussianConditional : gtsam::JacobianFactor { gtsam::Key parent2, const Vector& b, double sigma); - // Standard Interface + // Testable void print(string s = "GaussianConditional", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianConditional& cg, double tol) const; + + // Standard Interface gtsam::Key firstFrontalKey() const; + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues sample(const gtsam::VectorValues& parents) const; + gtsam::VectorValues sample() const; // Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; @@ -512,11 +516,13 @@ virtual class GaussianDensity : gtsam::GaussianConditional { const Vector& mean, double sigma); - // Standard Interface + // Testable void print(string s = "GaussianDensity", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianDensity& cg, double tol) const; + + // Standard Interface Vector mean() const; Matrix covariance() const; }; @@ -533,6 +539,21 @@ virtual class GaussianBayesNet { bool equals(const gtsam::GaussianBayesNet& other, double tol) const; size_t size() const; + // Standard interface + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues given) const; + gtsam::VectorValues optimizeGradientSearch() const; + + gtsam::VectorValues sample(gtsam::VectorValues given) const; + gtsam::VectorValues sample() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; + // FactorGraph derived interface gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; @@ -541,22 +562,12 @@ virtual class GaussianBayesNet { void saveGraph(const string& s) const; - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; std::pair matrix() const; - gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradientAtZero() const; double error(const gtsam::VectorValues& x) const; double determinant() const; double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; string dot( const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, From d2f6224b84ba984c6053c5c98bda236a7590a199 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 20:21:19 -0500 Subject: [PATCH 07/10] Add a static method to actually sample --- gtsam/linear/Sampler.cpp | 18 +++++++++++++----- gtsam/linear/Sampler.h | 12 ++++-------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 4957dfa14..20d4c955b 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -22,14 +22,18 @@ namespace gtsam { /* ************************************************************************* */ Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, uint_fast64_t seed) - : model_(model), generator_(seed) {} + : model_(model), generator_(seed) { + if (!model) { + throw std::invalid_argument("Sampler::Sampler needs a non-null model."); + } +} /* ************************************************************************* */ Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} /* ************************************************************************* */ -Vector Sampler::sampleDiagonal(const Vector& sigmas) const { +Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) { size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { @@ -39,14 +43,18 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) const { if (sigma == 0.0) { result(i) = 0.0; } else { - typedef std::normal_distribution Normal; - Normal dist(0.0, sigma); - result(i) = dist(generator_); + std::normal_distribution dist(0.0, sigma); + result(i) = dist(*rng); } } return result; } +/* ************************************************************************* */ +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { + return sampleDiagonal(sigmas, &generator_); +} + /* ************************************************************************* */ Vector Sampler::sample() const { assert(model_.get()); diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index bb5098f34..5be8b445d 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler { /// @name access functions /// @{ - size_t dim() const { - assert(model_.get()); - return model_->dim(); - } + size_t dim() const { return model_->dim(); } - Vector sigmas() const { - assert(model_.get()); - return model_->sigmas(); - } + Vector sigmas() const { return model_->sigmas(); } const noiseModel::Diagonal::shared_ptr& model() const { return model_; } @@ -82,6 +76,8 @@ class GTSAM_EXPORT Sampler { /// sample from distribution Vector sample() const; + /// sample with given random number generator + static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng); /// @} protected: From 13d370f61b183337cd0aebdc839fa0e5dcd7bc1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 20:21:47 -0500 Subject: [PATCH 08/10] Make all sampling methods take random number generator --- gtsam/linear/GaussianBayesNet.cpp | 21 ++++++++++++--- gtsam/linear/GaussianBayesNet.h | 21 +++++++++++++-- gtsam/linear/GaussianConditional.cpp | 22 ++++++++++++---- gtsam/linear/GaussianConditional.h | 26 +++++++++++++++---- gtsam/linear/tests/testGaussianBayesNet.cpp | 8 ++++++ .../linear/tests/testGaussianConditional.cpp | 13 +++++++--- 6 files changed, 92 insertions(+), 19 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 87b1c6cb4..6dcf662a9 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -26,6 +26,9 @@ using namespace std; using namespace gtsam; +// In Wrappers we have no access to this so have a default ready +static std::mt19937_64 kRandomNumberGenerator(42); + namespace gtsam { // Instantiate base class @@ -56,20 +59,30 @@ namespace gtsam { } /* ************************************************************************ */ - VectorValues GaussianBayesNet::sample() const { + VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const { VectorValues result; // no missing variables -> create an empty vector - return sample(result); + return sample(result, rng); } - VectorValues GaussianBayesNet::sample(VectorValues result) const { + VectorValues GaussianBayesNet::sample(VectorValues result, + std::mt19937_64* rng) const { // sample each node in reverse topological sort order (parents first) for (auto cg : boost::adaptors::reverse(*this)) { - const VectorValues sampled = cg->sample(result); + const VectorValues sampled = cg->sample(result, rng); result.insert(sampled); } return result; } + /* ************************************************************************ */ + VectorValues GaussianBayesNet::sample() const { + return sample(&kRandomNumberGenerator); + } + + VectorValues GaussianBayesNet::sample(VectorValues given) const { + return sample(given, &kRandomNumberGenerator); + } + /* ************************************************************************ */ VectorValues GaussianBayesNet::optimizeGradientSearch() const { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 18da3ed8a..940ffd882 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -95,10 +95,27 @@ namespace gtsam { /// Version of optimize for incomplete BayesNet, given missing variables VectorValues optimize(const VectorValues given) const; - /// Sample using ancestral sampling + /** + * Sample using ancestral sampling + * Example: + * std::mt19937_64 rng(42); + * auto sample = gbn.sample(&rng); + */ + VectorValues sample(std::mt19937_64* rng) const; + + /** + * Sample from an incomplete BayesNet, given missing variables + * Example: + * std::mt19937_64 rng(42); + * VectorValues given = ...; + * auto sample = gbn.sample(given, &rng); + */ + VectorValues sample(VectorValues given, std::mt19937_64* rng) const; + + /// Sample using ancestral sampling, use default rng VectorValues sample() const; - /// Sample from an incomplete BayesNet, given missing variables + /// Sample from an incomplete BayesNet, use default rng VectorValues sample(VectorValues given) const; /** diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 3aaffe6dc..7bc23bd45 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -35,6 +35,9 @@ #include #include +// In Wrappers we have no access to this so have a default ready +static std::mt19937_64 kRandomNumberGenerator(42); + using namespace std; namespace gtsam { @@ -222,8 +225,8 @@ namespace gtsam { } /* ************************************************************************ */ - VectorValues GaussianConditional::sample( - const VectorValues& parentsValues) const { + VectorValues GaussianConditional::sample(const VectorValues& parentsValues, + std::mt19937_64* rng) const { if (nrFrontals() != 1) { throw std::invalid_argument( "GaussianConditional::sample can only be called on single variable " @@ -235,13 +238,13 @@ namespace gtsam { "model was specified at construction."); } VectorValues solution = solve(parentsValues); - Sampler sampler(model_); Key key = firstFrontalKey(); - solution[key] += sampler.sample(); + const Vector& sigmas = model_->sigmas(); + solution[key] += Sampler::sampleDiagonal(sigmas, rng); return solution; } - VectorValues GaussianConditional::sample() const { + VectorValues GaussianConditional::sample(std::mt19937_64* rng) const { if (nrParents() != 0) throw std::invalid_argument( "sample() can only be invoked on no-parent prior"); @@ -249,6 +252,15 @@ namespace gtsam { return sample(values); } + /* ************************************************************************ */ + VectorValues GaussianConditional::sample() const { + return sample(&kRandomNumberGenerator); + } + + VectorValues GaussianConditional::sample(const VectorValues& given) const { + return sample(given, &kRandomNumberGenerator); + } + /* ************************************************************************ */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 void GTSAM_DEPRECATED diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e44da195b..e50928ba2 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -26,6 +26,8 @@ #include #include +#include // for std::mt19937_64 + namespace gtsam { /** @@ -150,15 +152,29 @@ namespace gtsam { void solveTransposeInPlace(VectorValues& gy) const; /** - * sample - * @param parentsValues Known values of the parents - * @return sample from conditional + * Sample from conditional, zero parent version + * Example: + * std::mt19937_64 rng(42); + * auto sample = gbn.sample(&rng); */ - VectorValues sample(const VectorValues& parentsValues) const; + VectorValues sample(std::mt19937_64* rng) const; - /// Zero parent version. + /** + * Sample from conditional, given missing variables + * Example: + * std::mt19937_64 rng(42); + * VectorValues given = ...; + * auto sample = gbn.sample(given, &rng); + */ + VectorValues sample(const VectorValues& parentsValues, + std::mt19937_64* rng) const; + + /// Sample, use default rng VectorValues sample() const; + /// Sample with given values, use default rng + VectorValues sample(const VectorValues& parentsValues) const; + /// @} #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index d9c6ee93d..b408ed5af 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -155,6 +155,14 @@ TEST(GaussianBayesNet, sample) { EXPECT_LONGS_EQUAL(2, actual.size()); EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma)); EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma)); + + // Use a specific random generator + std::mt19937_64 rng(4242); + auto actual3 = gbn.sample(&rng); + EXPECT_LONGS_EQUAL(2, actual.size()); + // regression: + EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5)); + EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index ae9a2d94b..973362cb1 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -352,14 +352,21 @@ TEST(GaussianConditional, sample) { EXPECT_LONGS_EQUAL(1, actual1.size()); EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma)); - VectorValues values; - values.insert(X(1), x1); + VectorValues given; + given.insert(X(1), x1); auto conditional = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); - auto actual2 = conditional.sample(values); + auto actual2 = conditional.sample(given); EXPECT_LONGS_EQUAL(1, actual2.size()); EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma)); + + // Use a specific random generator + std::mt19937_64 rng(4242); + auto actual3 = conditional.sample(given, &rng); + EXPECT_LONGS_EQUAL(1, actual2.size()); + // regression: + EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); } /* ************************************************************************* */ From 325613fc8e9f8514191a2501f896203438cda83b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Feb 2022 20:47:56 -0500 Subject: [PATCH 09/10] Fix test that used FromMeandAndStddev --- tests/testGaussianBayesTreeB.cpp | 40 ++++++++++++++------------------ 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index b8b6cf284..a321aa25d 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) C4 x7 : x6 ************************************************************************* */ -TEST( GaussianBayesTree, balanced_smoother_marginals ) -{ +TEST(GaussianBayesTree, balanced_smoother_marginals) { // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); VectorValues actualSolution = bayesTree.optimize(); VectorValues expectedSolution = VectorValues::Zero(actualSolution); - EXPECT(assert_equal(expectedSolution,actualSolution,tol)); + EXPECT(assert_equal(expectedSolution, actualSolution, tol)); - LONGS_EQUAL(4, (long)bayesTree.size()); + LONGS_EQUAL(4, bayesTree.size()); - double tol=1e-5; + double tol = 1e-5; // Check marginal on x1 - JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); - Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); - Matrix actualCovarianceX1; - GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); - actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); - EXPECT(assert_equal(expected1,actual1,tol)); + Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1); + auto m = bayesTree.marginalFactor(X(1), EliminateCholesky); + Matrix actualCovarianceX1 = m->information().inverse(); + EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol)); // Check marginal on x2 - double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2); + double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); - EXPECT(assert_equal(expected2,actual2,tol)); + Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2); + EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol)); // Check marginal on x3 - JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3); JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); - EXPECT(assert_equal(expected3,actual3,tol)); + Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3); + EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol)); // Check marginal on x4 - JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4); JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); - EXPECT(assert_equal(expected4,actual4,tol)); + Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4); + EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol)); // Check marginal on x7 (should be equal to x1) - JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7); JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); - EXPECT(assert_equal(expected7,actual7,tol)); + Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7); + EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol)); } /* ************************************************************************* */ From 4adba4fc6992c335208341f4341dec74608aa2da Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 7 Feb 2022 15:29:06 -0500 Subject: [PATCH 10/10] Comment out non-repeatable regressions --- gtsam/linear/tests/testGaussianBayesNet.cpp | 6 +++--- gtsam/linear/tests/testGaussianConditional.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index b408ed5af..2b125265f 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -160,9 +160,9 @@ TEST(GaussianBayesNet, sample) { std::mt19937_64 rng(4242); auto actual3 = gbn.sample(&rng); EXPECT_LONGS_EQUAL(2, actual.size()); - // regression: - EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5)); - EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5)); + // regression is not repeatable across platforms/versions :-( + // EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5)); + // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 973362cb1..df1b76d06 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -365,8 +365,8 @@ TEST(GaussianConditional, sample) { std::mt19937_64 rng(4242); auto actual3 = conditional.sample(given, &rng); EXPECT_LONGS_EQUAL(1, actual2.size()); - // regression: - EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); + // regression is not repeatable across platforms/versions :-( + // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); } /* ************************************************************************* */