Merge pull request #1894 from borglab/check-isam

gtsam/hybrid/HybridGaussianFactor.h

@@ -148,6 +148,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 
   /// Getter for GaussianFactor decision tree
   const FactorValuePairs &factors() const { return factors_; }
+
   /**
    * @brief Helper function to return factors and functional to create a
    * DecisionTree of Gaussian Factor Graphs.

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -581,4 +581,15 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose(
   return gfg;
 }
 
+/* ************************************************************************ */
+DiscreteFactorGraph HybridGaussianFactorGraph::discreteFactors() const {
+  DiscreteFactorGraph dfg;
+  for (auto &&f : factors_) {
+    if (auto discreteFactor = std::dynamic_pointer_cast<DiscreteFactor>(f)) {
+      dfg.push_back(discreteFactor);
+    }
+  }
+  return dfg;
+}
+
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
@@ -254,6 +255,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   GaussianFactorGraph operator()(const DiscreteValues& assignment) const {
     return choose(assignment);
   }
+
+  /**
+   * @brief Helper method to get all the discrete factors
+   * as a DiscreteFactorGraph.
+   *
+   * @return DiscreteFactorGraph
+   */
+  DiscreteFactorGraph discreteFactors() const;
 };
 
 // traits

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -16,6 +16,7 @@
  * @date   Sep 12, 2024
  */
 
+#include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -184,6 +185,11 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
       [continuousValues](
           const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair {
     auto [factor, val] = f;
+    // Check if valid factor. If not, return null and infinite error.
+    if (!factor) {
+      return {nullptr, std::numeric_limits<double>::infinity()};
+    }
+
     if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
             factor->noiseModel())) {
       return {factor->linearize(continuousValues),
@@ -202,4 +208,34 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
                                                 linearized_factors);
 }
 
-}  // namespace gtsam
+/* *******************************************************************************/
+HybridNonlinearFactor::shared_ptr HybridNonlinearFactor::prune(
+    const DecisionTreeFactor& discreteProbs) const {
+  // Find keys in discreteProbs.keys() but not in this->keys():
+  std::set<Key> mine(this->keys().begin(), this->keys().end());
+  std::set<Key> theirs(discreteProbs.keys().begin(),
+                       discreteProbs.keys().end());
+  std::vector<Key> diff;
+  std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
+                      std::back_inserter(diff));
+
+  // Find maximum probability value for every combination of our keys.
+  Ordering keys(diff);
+  auto max = discreteProbs.max(keys);
+
+  // Check the max value for every combination of our keys.
+  // If the max value is 0.0, we can prune the corresponding conditional.
+  auto pruner =
+      [&](const Assignment<Key>& choices,
+          const NonlinearFactorValuePair& pair) -> NonlinearFactorValuePair {
+    if (max->evaluate(choices) == 0.0)
+      return {nullptr, std::numeric_limits<double>::infinity()};
+    else
+      return pair;
+  };
+
+  FactorValuePairs prunedFactors = factors().apply(pruner);
+  return std::make_shared<HybridNonlinearFactor>(discreteKeys(), prunedFactors);
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactor.h

@@ -166,6 +166,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
 
   /// @}
 
+  /// Getter for NonlinearFactor decision tree
+  const FactorValuePairs& factors() const { return factors_; }
+
   /// Linearize specific nonlinear factors based on the assignment in
   /// discreteValues.
   GaussianFactor::shared_ptr linearize(
@@ -176,6 +179,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
   std::shared_ptr<HybridGaussianFactor> linearize(
       const Values& continuousValues) const;
 
+  /// Prune this factor based on the discrete probabilities.
+  HybridNonlinearFactor::shared_ptr prune(
+      const DecisionTreeFactor& discreteProbs) const;
+
  private:
   /// Helper struct to assist private constructor below.
   struct ConstructorHelper;

gtsam/hybrid/HybridNonlinearISAM.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridNonlinearFactor.h>
 #include <gtsam/hybrid/HybridNonlinearISAM.h>
 #include <gtsam/inference/Ordering.h>
 
@@ -39,7 +40,6 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
   if (newFactors.size() > 0) {
     // Reorder and relinearize every reorderInterval updates
     if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
-      // TODO(Varun) Re-linearization doesn't take into account pruning
       reorderRelinearize();
       reorderCounter_ = 0;
     }
@@ -65,8 +65,21 @@ void HybridNonlinearISAM::reorderRelinearize() {
     // Obtain the new linearization point
     const Values newLinPoint = estimate();
 
+    auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete());
+
     isam_.clear();
 
+    // Prune nonlinear factors based on discrete conditional probabilities
+    HybridNonlinearFactorGraph pruned_factors;
+    for (auto&& factor : factors_) {
+      if (auto nf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
+        pruned_factors.push_back(nf->prune(discreteProbs));
+      } else {
+        pruned_factors.push_back(factor);
+      }
+    }
+    factors_ = pruned_factors;
+
     // Just recreate the whole BayesTree
     // TODO: allow for constrained ordering here
     // TODO: decouple re-linearization and reordering to avoid

gtsam/hybrid/tests/Switching.h

@@ -52,7 +52,8 @@ using symbol_shorthand::X;
  * @return HybridGaussianFactorGraph::shared_ptr
  */
 inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
-    size_t K, std::function<Key(int)> x = X, std::function<Key(int)> m = M) {
+    size_t K, std::function<Key(int)> x = X, std::function<Key(int)> m = M,
+    const std::string &transitionProbabilityTable = "0 1 1 3") {
   HybridGaussianFactorGraph hfg;
 
   hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1));
@@ -68,7 +69,8 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
     hfg.add(HybridGaussianFactor({m(k), 2}, components));
 
     if (k > 1) {
-      hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, "0 1 1 3"));
+      hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}},
+                                 transitionProbabilityTable));
     }
   }
 

gtsam/hybrid/tests/testHybridBayesTree.cpp

@@ -74,9 +74,7 @@ TEST(HybridGaussianFactorGraph,
   hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
 
   hfg.add(DecisionTreeFactor(m1, {2, 8}));
-  // TODO(Varun) Adding extra discrete variable not connected to continuous
-  // variable throws segfault
-  //  hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4"));
+  hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4"));
 
   HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal();
 
@@ -176,7 +174,7 @@ TEST(HybridGaussianFactorGraph, Switching) {
   //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
   //     X(5),
   //                        M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
-  KeyVector ordering;
+  Ordering ordering;
 
   {
     std::vector<int> naturalX(N);
@@ -187,10 +185,6 @@ TEST(HybridGaussianFactorGraph, Switching) {
 
     auto [ndX, lvls] = makeBinaryOrdering(ordX);
     std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
-    // TODO(dellaert): this has no effect!
-    for (auto& l : lvls) {
-      l = -l;
-    }
   }
   {
     std::vector<int> naturalC(N - 1);
@@ -199,14 +193,11 @@ TEST(HybridGaussianFactorGraph, Switching) {
     std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
                    [](int x) { return M(x); });
 
-    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
     const auto [ndC, lvls] = makeBinaryOrdering(ordC);
     std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
   }
-  auto ordering_full = Ordering(ordering);
 
-  const auto [hbt, remaining] =
-      hfg->eliminatePartialMultifrontal(ordering_full);
+  const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering);
 
   // 12 cliques in the bayes tree and 0 remaining variables to eliminate.
   EXPECT_LONGS_EQUAL(12, hbt->size());
@@ -230,7 +221,7 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
   //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
   //     X(5),
   //                        M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
-  KeyVector ordering;
+  Ordering ordering;
 
   {
     std::vector<int> naturalX(N);
@@ -241,10 +232,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
 
     auto [ndX, lvls] = makeBinaryOrdering(ordX);
     std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
-    // TODO(dellaert): this has no effect!
-    for (auto& l : lvls) {
-      l = -l;
-    }
   }
   {
     std::vector<int> naturalC(N - 1);
@@ -257,10 +244,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
     const auto [ndC, lvls] = makeBinaryOrdering(ordC);
     std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
   }
-  auto ordering_full = Ordering(ordering);
 
-  const auto [hbt, remaining] =
-      hfg->eliminatePartialMultifrontal(ordering_full);
+  const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering);
 
   auto new_fg = makeSwitchingChain(12);
   auto isam = HybridGaussianISAM(*hbt);
@@ -460,12 +445,7 @@ TEST(HybridBayesTree, Optimize) {
   const auto [hybridBayesNet, remainingFactorGraph] =
       s.linearizedFactorGraph().eliminatePartialSequential(ordering);
 
-  DiscreteFactorGraph dfg;
-  for (auto&& f : *remainingFactorGraph) {
-    auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(f);
-    assert(discreteFactor);
-    dfg.push_back(discreteFactor);
-  }
+  DiscreteFactorGraph dfg = remainingFactorGraph->discreteFactors();
 
   // Add the probabilities for each branch
   DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}};