12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added zero parents FromMeanAndStddev

release/4.3a0
Frank Dellaert 2022-12-29 11:54:59 -05:00
parent 706a8a42bc
commit a6b90023f3
3 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/GaussianConditional.cpp
View File

@ -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));
}

9
gtsam/linear/GaussianConditional.h
View File

@ -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,

4
gtsam/linear/linear.i
View File

@ -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,