From 977ac0d7626be864c906248bdf8e72d0e2a4bd9e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 23 Oct 2024 09:24:47 -0700
Subject: [PATCH] Address review comments

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 7 ++-----
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 4 +++-
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index a890b37b75..c60ab47aaf 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -275,9 +275,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
         if (auto conditional =
                 std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
           const auto likelihood_m = conditional->likelihood(given);
-          // scalar is already correct.
-          assert(pair.second ==
-                 conditional->negLogConstant() - negLogConstant_);
+          // pair.second == conditional->negLogConstant() - negLogConstant_
           return {likelihood_m, pair.second};
         } else {
           return {nullptr, std::numeric_limits<double>::infinity()};
@@ -320,8 +318,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
   };
 
   FactorValuePairs prunedConditionals = factors().apply(pruner);
-  return std::shared_ptr<HybridGaussianConditional>(
-      new HybridGaussianConditional(discreteKeys(), prunedConditionals));
+  return std::make_shared<HybridGaussianConditional>(discreteKeys(), prunedConditionals);
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index bc3856fe7f..ecc317ab3a 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -367,7 +367,7 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
   // any difference in noise models used.
   HybridGaussianProductFactor productFactor = collectProductFactor();
 
-  auto isNull = [](const GaussianFactor::shared_ptr& ptr) { return !ptr; };
+  auto isNull = [](const GaussianFactor::shared_ptr &ptr) { return !ptr; };
 
   // This is the elimination method on the leaf nodes
   bool someContinuousLeft = false;
@@ -375,6 +375,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
       [&](const std::pair<GaussianFactorGraph, double> &pair) -> Result {
     const auto &[graph, scalar] = pair;
 
+    // If any product contains a pruned factor, prune it here. Done here as it's
+    // non non-trivial to do within collectProductFactor.
     if (graph.empty() || std::any_of(graph.begin(), graph.end(), isNull)) {
       return {nullptr, 0.0, nullptr, 0.0};
     }