Merge branch 'working-hybrid' into direct-hybrid-fg

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -593,6 +593,55 @@ TEST(ADT, zero) {
   EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9);
 }
 
+/// Example ADT from 0 to 11.
+ADT exampleADT() {
+  DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+  ADT f(A & B & C, "0 6  2 8  4 10    1 7  3 9  5 11");
+  return f;
+}
+/* ************************************************************************** */
+// Test sum
+TEST(ADT, Sum) {
+  ADT a = exampleADT();
+  double expected_sum = 0;
+  for (double i = 0; i < 12; i++) {
+    expected_sum += i;
+  }
+  EXPECT_DOUBLES_EQUAL(expected_sum, a.sum(), 1e-9);
+}
+
+/* ************************************************************************** */
+// Test normalize
+TEST(ADT, Normalize) {
+  ADT a = exampleADT();
+  double sum = a.sum();
+  auto actual = a.normalize(sum);
+
+  DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+  DiscreteKeys keys = DiscreteKeys{A, B, C};
+  std::vector<double> cpt{0 / sum, 6 / sum,  2 / sum, 8 / sum,
+                          4 / sum, 10 / sum, 1 / sum, 7 / sum,
+                          3 / sum, 9 / sum,  5 / sum, 11 / sum};
+  ADT expected(keys, cpt);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************** */
+// Test min
+TEST(ADT, Min) {
+  ADT a = exampleADT();
+  double min = a.min();
+  EXPECT_DOUBLES_EQUAL(0.0, min, 1e-9);
+}
+
+/* ************************************************************************** */
+// Test max
+TEST(ADT, Max) {
+  ADT a = exampleADT();
+  double max = a.max();
+  EXPECT_DOUBLES_EQUAL(11.0, max, 1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/geometry.i

@@ -856,7 +856,7 @@ class Cal3_S2Stereo {
   gtsam::Matrix K() const;
   gtsam::Point2 principalPoint() const;
   double baseline() const;
-  Vector6 vector() const;
+  gtsam::Vector6 vector() const;
   gtsam::Matrix inverse() const;
 };
 

gtsam/hybrid/GaussianMixture.cpp

@@ -200,27 +200,24 @@ std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
   const GaussianMixtureFactor::Factors likelihoods(
       conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
         const auto likelihood_m = conditional->likelihood(given);
+        const double Cgm_Kgcm =
+            logConstant_ - conditional->logNormalizationConstant();
+        if (Cgm_Kgcm == 0.0) {
           return likelihood_m;
-      });
+        } else {
-
+          // Add a constant factor to the likelihood in case the noise models
-  // First compute all the sqrt(|2 pi Sigma|) terms
+          // are not all equal.
-  auto computeLogNormalizers = [](const GaussianFactor::shared_ptr &gf) {
+          GaussianFactorGraph gfg;
-    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
+          gfg.push_back(likelihood_m);
-    // If we have, say, a Hessian factor, then no need to do anything
+          Vector c(1);
-    if (!jf) return 0.0;
+          c << std::sqrt(2.0 * Cgm_Kgcm);
-
+          auto constantFactor = std::make_shared<JacobianFactor>(c);
-    auto model = jf->get_model();
+          gfg.push_back(constantFactor);
-    // If there is no noise model, there is nothing to do.
+          return std::make_shared<JacobianFactor>(gfg);
-    if (!model) {
-      return 0.0;
         }
-    return ComputeLogNormalizer(model);
+      });
-  };
-
-  AlgebraicDecisionTree<Key> log_normalizers =
-      DecisionTree<Key, double>(likelihoods, computeLogNormalizers);
   return std::make_shared<GaussianMixtureFactor>(
-      continuousParentKeys, discreteParentKeys, likelihoods, log_normalizers);
+      continuousParentKeys, discreteParentKeys, likelihoods);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -28,55 +28,11 @@
 
 namespace gtsam {
 
-/**
- * @brief Helper function to augment the [A|b] matrices in the factor components
- * with the normalizer values.
- * This is done by storing the normalizer value in
- * the `b` vector as an additional row.
- *
- * @param factors DecisionTree of GaussianFactor shared pointers.
- * @param logNormalizers Tree of log-normalizers corresponding to each
- * Gaussian factor in factors.
- * @return GaussianMixtureFactor::Factors
- */
-GaussianMixtureFactor::Factors augment(
-    const GaussianMixtureFactor::Factors &factors,
-    const AlgebraicDecisionTree<Key> &logNormalizers) {
-  // Find the minimum value so we can "proselytize" to positive values.
-  // Done because we can't have sqrt of negative numbers.
-  double min_log_normalizer = logNormalizers.min();
-  AlgebraicDecisionTree<Key> log_normalizers = logNormalizers.apply(
-      [&min_log_normalizer](double n) { return n - min_log_normalizer; });
-
-  // Finally, update the [A|b] matrices.
-  auto update = [&log_normalizers](
-                    const Assignment<Key> &assignment,
-                    const GaussianMixtureFactor::sharedFactor &gf) {
-    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
-    if (!jf) return gf;
-    // If the log_normalizer is 0, do nothing
-    if (log_normalizers(assignment) == 0.0) return gf;
-
-    GaussianFactorGraph gfg;
-    gfg.push_back(jf);
-
-    Vector c(1);
-    c << std::sqrt(log_normalizers(assignment));
-    auto constantFactor = std::make_shared<JacobianFactor>(c);
-
-    gfg.push_back(constantFactor);
-    return std::dynamic_pointer_cast<GaussianFactor>(
-        std::make_shared<JacobianFactor>(gfg));
-  };
-  return factors.apply(update);
-}
-
 /* *******************************************************************************/
-GaussianMixtureFactor::GaussianMixtureFactor(
+GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
-    const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+                                             const DiscreteKeys &discreteKeys,
-    const Factors &factors, const AlgebraicDecisionTree<Key> &logNormalizers)
+                                             const Factors &factors)
-    : Base(continuousKeys, discreteKeys),
+    : Base(continuousKeys, discreteKeys), factors_(factors) {}
-      factors_(augment(factors, logNormalizers)) {}
 
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
@@ -164,20 +120,4 @@ double GaussianMixtureFactor::error(const HybridValues &values) const {
   return gf->error(values.continuous());
 }
 
-/* *******************************************************************************/
-double ComputeLogNormalizer(
-    const noiseModel::Gaussian::shared_ptr &noise_model) {
-  // Since noise models are Gaussian, we can get the logDeterminant using
-  // the same trick as in GaussianConditional
-  double logDetR = noise_model->R()
-                       .diagonal()
-                       .unaryExpr([](double x) { return log(x); })
-                       .sum();
-  double logDeterminantSigma = -2.0 * logDetR;
-
-  size_t n = noise_model->dim();
-  constexpr double log2pi = 1.8378770664093454835606594728112;
-  return n * log2pi + logDeterminantSigma;
-}
-
 }  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.h

@@ -82,14 +82,10 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * their cardinalities.
    * @param factors The decision tree of Gaussian factors stored as the mixture
    * density.
-   * @param logNormalizers Tree of log-normalizers corresponding to each
-   * Gaussian factor in factors.
    */
   GaussianMixtureFactor(const KeyVector &continuousKeys,
                         const DiscreteKeys &discreteKeys,
-                        const Factors &factors,
+                        const Factors &factors);
-                        const AlgebraicDecisionTree<Key> &logNormalizers =
-                            AlgebraicDecisionTree<Key>(0.0));
 
   /**
    * @brief Construct a new GaussianMixtureFactor object using a vector of
@@ -98,16 +94,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
    * @param factors Vector of gaussian factor shared pointers.
-   * @param logNormalizers Tree of log-normalizers corresponding to each
-   * Gaussian factor in factors.
    */
   GaussianMixtureFactor(const KeyVector &continuousKeys,
                         const DiscreteKeys &discreteKeys,
-                        const std::vector<sharedFactor> &factors,
+                        const std::vector<sharedFactor> &factors)
-                        const AlgebraicDecisionTree<Key> &logNormalizers =
-                            AlgebraicDecisionTree<Key>(0.0))
       : GaussianMixtureFactor(continuousKeys, discreteKeys,
-                              Factors(discreteKeys, factors), logNormalizers) {}
+                              Factors(discreteKeys, factors)) {}
 
   /// @}
   /// @name Testable

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -80,6 +80,8 @@ TEST(GaussianBayesNet, Evaluate1) {
                        smallBayesNet.at(0)->logNormalizationConstant() +
                            smallBayesNet.at(1)->logNormalizationConstant(),
                        1e-9);
+  EXPECT_DOUBLES_EQUAL(log(constant), smallBayesNet.logNormalizationConstant(),
+                       1e-9);
   const double actual = smallBayesNet.evaluate(mean);
   EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
 }