Debug/city10000

examples/ISAM2_City10000.cpp

@@ -165,7 +165,8 @@ int main(int argc, char* argv[]) {
     // Print loop index and time taken in processor clock ticks
     if (index % 50 == 0 && key_s != key_t - 1) {
       std::cout << "index: " << index << std::endl;
-      std::cout << "acc_time:  " << time_list.back() << std::endl;
+      std::cout << "acc_time:  " << time_list.back() / CLOCKS_PER_SEC
+                << std::endl;
     }
 
     if (key_s == key_t - 1) {
@@ -190,7 +191,7 @@ int main(int argc, char* argv[]) {
 
   clock_t end_time = clock();
   clock_t total_time = end_time - start_time;
-  cout << "total_time: " << total_time << endl;
+  cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl;
 
   /// Write results to file
   write_results(results, (key_t + 1));

gtsam/base/SymmetricBlockMatrix.h

@@ -256,11 +256,16 @@ namespace gtsam {
       full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
     }
 
-    /// Set the entire active matrix zero.
+    /// Set the entire *active* matrix zero.
     void setZero() {
       full().triangularView<Eigen::Upper>().setZero();
     }
 
+    /// Set entire matrix zero.
+    void setAllZero() {
+      matrix_.setZero();
+    }
+
     /// Negate the entire active matrix.
     void negate();
 

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -228,11 +228,11 @@ namespace gtsam {
     // We divide both `product` and `sum` by `max(sum)`
     // since it is faster to compute and when the conditional
     // is formed by `product/sum`, the scaling term cancels out.
-    gttic(scale);
-    DiscreteFactor::shared_ptr denominator = sum->max(sum->size());
-    product = product->operator/(denominator);
-    sum = sum->operator/(denominator);
-    gttoc(scale);
+    // gttic(scale);
+    // DiscreteFactor::shared_ptr denominator = sum->max(sum->size());
+    // product = product->operator/(denominator);
+    // sum = sum->operator/(denominator);
+    // gttoc(scale);
 
     // Ordering keys for the conditional so that frontalKeys are really in front
     Ordering orderedKeys;

gtsam/hybrid/HybridBayesNet.cpp

@@ -49,6 +49,9 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
 // search to find the K-best leaves and then create a single pruned conditional.
 HybridBayesNet HybridBayesNet::prune(
     size_t maxNrLeaves, const std::optional<double> &deadModeThreshold) const {
+#if GTSAM_HYBRID_TIMING
+  gttic_(HybridPruning);
+#endif
   // Collect all the discrete conditionals. Could be small if already pruned.
   const DiscreteBayesNet marginal = discreteMarginal();
 
@@ -69,6 +72,10 @@ HybridBayesNet HybridBayesNet::prune(
   // If we have a dead mode threshold and discrete variables left after pruning,
   // then we run dead mode removal.
   if (deadModeThreshold.has_value() && pruned.keys().size() > 0) {
+#if GTSAM_HYBRID_TIMING
+    gttic_(DeadModeRemoval);
+#endif
+
     DiscreteMarginals marginals(DiscreteFactorGraph{pruned});
     for (auto dkey : pruned.discreteKeys()) {
       Vector probabilities = marginals.marginalProbabilities(dkey);
@@ -89,24 +96,11 @@ HybridBayesNet HybridBayesNet::prune(
     // Remove the modes (imperative)
     pruned.removeDiscreteModes(deadModesValues);
 
-    /*
-      If the pruned discrete conditional has any keys left,
-      we add it to the HybridBayesNet.
-      If not, it means it is an orphan so we don't add this pruned joint,
-      and instead add only the marginals below.
-    */
-    if (pruned.keys().size() > 0) {
-      result.emplace_shared<DiscreteConditional>(pruned);
-    }
-
-    // Add the marginals for future factors
-    for (auto &&[key, _] : deadModesValues) {
-      result.push_back(
-          std::dynamic_pointer_cast<DiscreteConditional>(marginals(key)));
-    }
+    GTSAM_PRINT(deadModesValues);
 
-  } else {
-    result.emplace_shared<DiscreteConditional>(pruned);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(DeadModeRemoval);
+#endif
   }
 
   /* To prune, we visitWith every leaf in the HybridGaussianConditional.
@@ -122,20 +116,37 @@ HybridBayesNet HybridBayesNet::prune(
     if (auto hgc = conditional->asHybrid()) {
       // Prune the hybrid Gaussian conditional!
       auto prunedHybridGaussianConditional = hgc->prune(pruned);
+      if (!prunedHybridGaussianConditional) {
+        GTSAM_PRINT(marginal);
+        GTSAM_PRINT(pruned);
+        throw std::runtime_error(
+            "A HybridGaussianConditional had all its conditionals pruned");
+      }
 
       if (deadModeThreshold.has_value()) {
-        KeyVector deadKeys, conditionalDiscreteKeys;
-        for (const auto &kv : deadModesValues) {
-          deadKeys.push_back(kv.first);
-        }
-        for (auto dkey : prunedHybridGaussianConditional->discreteKeys()) {
-          conditionalDiscreteKeys.push_back(dkey.first);
+        const auto &discreteParents =
+            prunedHybridGaussianConditional->discreteKeys();
+        DiscreteValues deadParentValues;
+        DiscreteKeys liveParents;
+        for (const auto &key : discreteParents) {
+          auto it = deadModesValues.find(key.first);
+          if (it != deadModesValues.end())
+            deadParentValues[key.first] = it->second;
+          else
+            liveParents.emplace_back(key);
         }
-        // The discrete keys in the conditional are the same as the keys in the
-        // dead modes, then we just get the corresponding Gaussian conditional.
-        if (deadKeys == conditionalDiscreteKeys) {
+        // If so then we just get the corresponding Gaussian conditional:
+        if (deadParentValues.size() == discreteParents.size()) {
+          // print on how many discreteParents we are choosing:
           result.push_back(
-              prunedHybridGaussianConditional->choose(deadModesValues));
+              prunedHybridGaussianConditional->choose(deadParentValues));
+        } else if (liveParents.size() > 0) {
+          auto newTree = prunedHybridGaussianConditional->factors();
+          for (auto &&[key, value] : deadModesValues) {
+            newTree = newTree.choose(key, value);
+          }
+          result.emplace_shared<HybridGaussianConditional>(liveParents,
+                                                           newTree);
         } else {
           // Add as-is
           result.push_back(prunedHybridGaussianConditional);
@@ -152,6 +163,31 @@ HybridBayesNet HybridBayesNet::prune(
     // We ignore DiscreteConditional as they are already pruned and added.
   }
 
+#if GTSAM_HYBRID_TIMING
+  gttoc_(HybridPruning);
+#endif
+
+  if (deadModeThreshold.has_value()) {
+    /*
+      If the pruned discrete conditional has any keys left,
+      we add it to the HybridBayesNet.
+      If not, it means it is an orphan so we don't add this pruned joint,
+      and instead add only the marginals below.
+    */
+    if (pruned.keys().size() > 0) {
+      result.emplace_shared<DiscreteConditional>(pruned);
+    }
+
+    // Add the marginals for future factors
+    // for (auto &&[key, _] : deadModesValues) {
+    //   result.push_back(
+    //       std::dynamic_pointer_cast<DiscreteConditional>(marginals(key)));
+    // }
+
+  } else {
+    result.emplace_shared<DiscreteConditional>(pruned);
+  }
+
   return result;
 }
 

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -313,25 +313,29 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
   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);
+  // Find maximum probability value for every combination of *our* keys.
+  Ordering ordering(diff);
+  auto max = discreteProbs.max(ordering);
 
   // Check the max value for every combination of our keys.
   // If the max value is 0.0, we can prune the corresponding conditional.
+  bool allPruned = true;
   auto pruner =
       [&](const Assignment<Key> &choices,
           const GaussianFactorValuePair &pair) -> GaussianFactorValuePair {
-    if (max->evaluate(choices) == 0.0)
+    // If this choice is zero probability or Gaussian is null, return infinity
+    if (!pair.first || max->evaluate(choices) == 0.0) {
       return {nullptr, std::numeric_limits<double>::infinity()};
-    else {
+    } else {
+      allPruned = false;
       // Add negLogConstant_ back so that the minimum negLogConstant in the
       // HybridGaussianConditional is set correctly.
       return {pair.first, pair.second + negLogConstant_};
     }
   };
 
   FactorValuePairs prunedConditionals = factors().apply(pruner);
+  if (allPruned) return nullptr;
   return std::make_shared<HybridGaussianConditional>(discreteKeys(),
                                                      prunedConditionals, true);
 }

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -50,6 +50,8 @@
 #include <utility>
 #include <vector>
 
+#define GTSAM_HYBRID_WITH_TABLEFACTOR 0
+
 namespace gtsam {
 
 /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph:
@@ -253,7 +255,11 @@ static DiscreteFactor::shared_ptr DiscreteFactorFromErrors(
   double min_log = errors.min();
   AlgebraicDecisionTree<Key> potentials(
       errors, [&min_log](const double x) { return exp(-(x - min_log)); });
+#if GTSAM_HYBRID_WITH_TABLEFACTOR
   return std::make_shared<TableFactor>(discreteKeys, potentials);
+#else
+  return std::make_shared<DecisionTreeFactor>(discreteKeys, potentials);
+#endif
 }
 
 /* ************************************************************************ */
@@ -290,9 +296,13 @@ static DiscreteFactorGraph CollectDiscreteFactors(
         /// Get the underlying TableFactor
         dfg.push_back(dtc->table());
       } else {
+#if GTSAM_HYBRID_WITH_TABLEFACTOR
         // Convert DiscreteConditional to TableFactor
         auto tdc = std::make_shared<TableFactor>(*dc);
         dfg.push_back(tdc);
+#else
+        dfg.push_back(dc);
+#endif
       }
 #if GTSAM_HYBRID_TIMING
       gttoc_(ConvertConditionalToTableFactor);
@@ -309,11 +319,18 @@ static DiscreteFactorGraph CollectDiscreteFactors(
 static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
 discreteElimination(const HybridGaussianFactorGraph &factors,
                     const Ordering &frontalKeys) {
+#if GTSAM_HYBRID_TIMING
+  gttic_(CollectDiscreteFactors);
+#endif
   DiscreteFactorGraph dfg = CollectDiscreteFactors(factors);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(CollectDiscreteFactors);
+#endif
 
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscrete);
 #endif
+#if GTSAM_HYBRID_WITH_TABLEFACTOR
   // Check if separator is empty.
   // This is the same as checking if the number of frontal variables
   // is the same as the number of variables in the DiscreteFactorGraph.
@@ -323,9 +340,6 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     // Get product factor
     DiscreteFactor::shared_ptr product = dfg.scaledProduct();
 
-#if GTSAM_HYBRID_TIMING
-    gttic_(EliminateDiscreteFormDiscreteConditional);
-#endif
     // Check type of product, and get as TableFactor for efficiency.
     // Use object instead of pointer since we need it
     // for the TableDistribution constructor.
@@ -337,19 +351,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     }
     auto conditional = std::make_shared<TableDistribution>(p);
 
-#if GTSAM_HYBRID_TIMING
-    gttoc_(EliminateDiscreteFormDiscreteConditional);
-#endif
-
     DiscreteFactor::shared_ptr sum = p.sum(frontalKeys);
 
     return {std::make_shared<HybridConditional>(conditional), sum};
 
   } else {
+#endif
     // Perform sum-product.
     auto result = EliminateDiscrete(dfg, frontalKeys);
     return {std::make_shared<HybridConditional>(result.first), result.second};
+#if GTSAM_HYBRID_WITH_TABLEFACTOR
   }
+#endif
 #if GTSAM_HYBRID_TIMING
   gttoc_(EliminateDiscrete);
 #endif
@@ -411,8 +424,14 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
       throw std::runtime_error("createHybridGaussianFactors has mixed NULLs");
     }
   };
+#if GTSAM_HYBRID_TIMING
+  gttic_(HybridCreateGaussianFactor);
+#endif
   DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
                                                         correct);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(HybridCreateGaussianFactor);
+#endif
 
   return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors);
 }