likelihood method for GaussianConditionals

gtsam/discrete/DiscreteConditional.cpp

@@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
 
 /* ****************************************************************************/
 DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
-    size_t parent_value) const {
+    size_t frontal) const {
   if (nrFrontals() != 1)
     throw std::invalid_argument(
         "Single value likelihood can only be invoked on single-variable "
         "conditional");
   DiscreteValues values;
-  values.emplace(keys_[0], parent_value);
+  values.emplace(keys_[0], frontal);
   return likelihood(values);
 }
 

gtsam/discrete/DiscreteConditional.h

@@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional
       const DiscreteValues& frontalValues) const;
 
   /** Single variable version of likelihood. */
-  DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
+  DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
 
   /**
    * sample

gtsam/linear/GaussianConditional.cpp

@@ -224,6 +224,50 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************ */
+  JacobianFactor::shared_ptr GaussianConditional::likelihood(
+      const VectorValues& frontalValues) const {
+    // Error is |Rx - (d - Sy - Tz - ...)|^2
+    // so when we instantiate x (which has to be completely known) we beget:
+    // |Sy + Tz + ... - (d - Rx)|^2
+    // The noise model just transfers over!
+
+    // Get frontalValues as vector
+    const Vector x =
+        frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));
+
+    // Copy the augmented Jacobian matrix:
+    auto newAb = Ab_;
+
+    // Restrict view to parent blocks
+    newAb.firstBlock() += nrFrontals_;
+
+    // Update right-hand-side (last column)
+    auto last = newAb.matrix().cols() - 1;
+    const auto RR = R().triangularView<Eigen::Upper>();
+    newAb.matrix().col(last) -= RR * x;
+
+    // The keys now do not include the frontal keys:
+    KeyVector newKeys;
+    newKeys.reserve(nrParents());
+    for (auto&& key : parents()) newKeys.push_back(key);
+
+    // Hopefully second newAb copy below is optimized out...
+    return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
+  }
+
+  /* **************************************************************************/
+  JacobianFactor::shared_ptr GaussianConditional::likelihood(
+      const Vector& frontal) const {
+    if (nrFrontals() != 1)
+      throw std::invalid_argument(
+          "GaussianConditional Single value likelihood can only be invoked on "
+          "single-variable conditional");
+    VectorValues values;
+    values.insert(keys_[0], frontal);
+    return likelihood(values);
+  }
+
   /* ************************************************************************ */
   VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
                                            std::mt19937_64* rng) const {

gtsam/linear/GaussianConditional.h

@@ -151,6 +151,13 @@ namespace gtsam {
     /** Performs transpose backsubstition in place on values */
     void solveTransposeInPlace(VectorValues& gy) const;
 
+    /** Convert to a likelihood factor by providing value before bar. */
+    JacobianFactor::shared_ptr likelihood(
+        const VectorValues& frontalValues) const;
+
+    /** Single variable version of likelihood. */
+    JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;
+
     /**
      * Sample from conditional, zero parent version
      * Example:

gtsam/linear/linear.i

@@ -255,7 +255,7 @@ class VectorValues {
 
   // enabling serialization functionality
   void serialize() const;
-  void html() const;
+  string html() const;
 };
 
 #include <gtsam/linear/GaussianFactor.h>
@@ -492,6 +492,9 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   // Standard Interface
   gtsam::Key firstFrontalKey() const;
   gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
+  gtsam::JacobianFactor* likelihood(
+      const gtsam::VectorValues& frontalValues) const;
+  gtsam::JacobianFactor* likelihood(Vector frontal) const;
   gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
   gtsam::VectorValues sample() const;
   

gtsam/linear/tests/testGaussianConditional.cpp

@@ -340,6 +340,33 @@ TEST(GaussianConditional, FromMeanAndStddev) {
   EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
 }
 
+/* ************************************************************************* */
+// Test likelihood method (conversion to JacobianFactor)
+TEST(GaussianConditional, likelihood) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x0(1, 2);
+  const double sigma = 0.01;
+
+  // |x0 - A1 x1 - b|^2
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+
+  VectorValues frontalValues;
+  frontalValues.insert(X(0), x0);
+  auto actual1 = conditional.likelihood(frontalValues);
+  CHECK(actual1);
+
+  // |(-A1) x1 - (b - x0)|^2
+  JacobianFactor expected(X(1), -A1, b - x0,
+                          noiseModel::Isotropic::Sigma(2, sigma));
+  EXPECT(assert_equal(expected, *actual1, tol));
+
+  // Check single vector version
+  auto actual2 = conditional.likelihood(x0);
+  CHECK(actual2);
+  EXPECT(assert_equal(expected, *actual2, tol));
+}
+
 /* ************************************************************************* */
 // Test sampling
 TEST(GaussianConditional, sample) {