diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 5e8a193cf..7cdff914f 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -63,13 +63,24 @@ namespace gtsam { : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas), BaseConditional(1) {} + /* ************************************************************************ */ + GaussianConditional GaussianConditional::FromMeanAndStddev(Key key, + const Vector& mu, + double sigma) { + // |Rx - d| = |x-(Ay + b)|/sigma + const Matrix R = Matrix::Identity(mu.size(), mu.size()); + const Vector& d = mu; + return GaussianConditional(key, d, R, + noiseModel::Isotropic::Sigma(mu.size(), sigma)); + } + /* ************************************************************************ */ 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()); const Matrix S = -A; - const Vector d = b; + const Vector& d = b; return GaussianConditional(key, d, R, parent, S, noiseModel::Isotropic::Sigma(b.size(), sigma)); } @@ -82,7 +93,7 @@ namespace gtsam { const Matrix R = Matrix::Identity(b.size(), b.size()); const Matrix S = -A1; const Matrix T = -A2; - const Vector d = b; + const Vector& d = b; return GaussianConditional(key, d, R, parent1, S, parent2, T, noiseModel::Isotropic::Sigma(b.size(), sigma)); } diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index a72a73973..af1c5d80e 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -84,12 +84,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. + /// Construct from mean `mu` and standard deviation `sigma`. + static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu, + double sigma); + + /// Construct from conditional 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. + /// Construct from conditional mean `A1 p1 + A2 p2 + b` and standard + /// deviation `sigma`. static GaussianConditional FromMeanAndStddev(Key key, // const Matrix& A1, Key parent1, const Matrix& A2, Key parent2, diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f5857b0c5..6f241da55 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -470,6 +470,10 @@ virtual class GaussianConditional : gtsam::JacobianFactor { size_t name2, Matrix T); // Named constructors + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, + const Vector& mu, + double sigma); + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, const Matrix& A, gtsam::Key parent,