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