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