Working with sigma in matrices

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(
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
-    Key key, const Vector& d, const Matrix& R,
+                                           const Matrix& R, Key parent1,
-    Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
+                                           const Matrix& S,
-  BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
+                                           const SharedDiagonal& sigmas)
+      : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
-  GaussianConditional::GaussianConditional(
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
-    Key key, const Vector& d, const Matrix& R,
+                                           const Matrix& R, Key parent1,
-    Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
+                                           const Matrix& S, Key parent2,
-  BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
+                                           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) {

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,12 +52,13 @@ namespace gtsam {
       const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with only one parent |Rx+Sy-d| */
-    GaussianConditional(Key key, const Vector& d, const Matrix& R,
+    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
-      Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
+                        const Matrix& S,
+                        const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with two parents |Rx+Sy+Tz-d| */
-    GaussianConditional(Key key, const Vector& d, const Matrix& R,
+    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
-      Key name1, const Matrix& S, Key name2, const Matrix& T,
+                        const Matrix& S, Key parent2, const Matrix& T,
                         const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Constructor with arbitrary number of frontals and parents.
@@ -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.

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 =

gtsam/linear/tests/testGaussianConditional.cpp

@@ -20,7 +20,7 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/VerticalBlockMatrix.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianBayesNet.h>
@@ -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);
+}
 /* ************************************************************************* */