add HybridGaussianFactorGraph::probPrime method

borglab · Nov 8, 2022 · eb94ad9 · eb94ad9
1 parent cb55af3
commit eb94ad9
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 1 deletion.
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -511,6 +511,14 @@ double HybridGaussianFactorGraph::error(
   return error;
 }
 
+/* ************************************************************************ */
+double HybridGaussianFactorGraph::probPrime(
+    const VectorValues &continuousValues,
+    const DiscreteValues &discreteValues) const {
+  double error = this->error(continuousValues, discreteValues);
+  return std::exp(-error);
+}
+
 /* ************************************************************************ */
 AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime(
     const VectorValues &continuousValues) const {

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -204,6 +204,18 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   AlgebraicDecisionTree<Key> probPrime(
       const VectorValues& continuousValues) const;
 
+  /**
+   * @brief Compute the unnormalized posterior probability for a continuous
+   * vector values given a specific assignment.
+   *
+   * @param continuousValues The vector values for which to compute the
+   * posterior probability.
+   * @param discreteValues The specific assignment to use for the computation.
+   * @return double
+   */
+  double probPrime(const VectorValues& continuousValues,
+                   const DiscreteValues& discreteValues) const;
+
   /**
    * @brief Compute the VectorValues solution for the continuous variables for
    * each mode.

diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -581,7 +581,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
   EXPECT(assert_equal(expected_error, error, 1e-9));
 
   double probs = exp(-error);
-  double expected_probs = exp(-expected_error);
+  double expected_probs = graph.probPrime(delta.continuous(), delta.discrete());
 
   // regression
   EXPECT(assert_equal(expected_probs, probs, 1e-7));