diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 52f1a3f0a5..cbf3bc376d 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -299,19 +299,19 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixture::logProbability( const VectorValues &continuousValues) const { - // functor to calculate to double logProbability value from + // functor to calculate (double) logProbability value from // GaussianConditional. - auto errorFunc = + auto probFunc = [continuousValues](const GaussianConditional::shared_ptr &conditional) { if (conditional) { return conditional->logProbability(continuousValues); } else { - // Return arbitrarily large logProbability if conditional is null + // Return arbitrarily small logProbability if conditional is null // Conditional is null if it is pruned out. - return 1e50; + return -1e20; } }; - return DecisionTree(conditionals_, errorFunc); + return DecisionTree(conditionals_, probFunc); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 772fb250c0..7c9023e582 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -76,13 +76,16 @@ std::function &, double)> prunerFunc( auto pruner = [prunedDecisionTree, decisionTreeKeySet, conditionalKeySet]( const Assignment &choices, double probability) -> double { + // This corresponds to 0 probability + double pruned_prob = 0.0; + // typecast so we can use this to get probability value DiscreteValues values(choices); // Case where the Gaussian mixture has the same // discrete keys as the decision tree. if (conditionalKeySet == decisionTreeKeySet) { if (prunedDecisionTree(values) == 0) { - return 0.0; + return pruned_prob; } else { return probability; } @@ -133,7 +136,7 @@ std::function &, double)> prunerFunc( } // If we are here, it means that all the sub-branches are 0, // so we prune. - return 0.0; + return pruned_prob; } }; return pruner; diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index d6b83e30d6..b7a5a8eeb3 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -39,6 +39,7 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { + // TODO(Varun) Relinearization doesn't take into account pruning reorder_relinearize(); reorderCounter_ = 0; }