diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 06b2856f8..0d6c5e976 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/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); } diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 48d94a383..cff1b69a6 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/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 diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 7bc23bd45..cf3f78b73 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/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(); + 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(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 { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e50928ba2..6dd278536 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/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: diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3f3f79b14..f1bc92f69 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -255,7 +255,7 @@ class VectorValues { // enabling serialization functionality void serialize() const; - void html() const; + string html() const; }; #include @@ -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; diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index df1b76d06..8525cf9e0 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/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) {