diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index e03e58f7a..b5344a988 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -65,7 +65,7 @@ static GaussianMixtureFactor::Sum &addGaussian( if (sum.empty()) { GaussianFactorGraph result; result.push_back(factor); - sum = GaussianMixtureFactor::Sum( + return GaussianMixtureFactor::Sum( GaussianMixtureFactor::GraphAndConstant(result, 0.0)); } else { @@ -75,12 +75,19 @@ static GaussianMixtureFactor::Sum &addGaussian( result.push_back(factor); return GaussianMixtureFactor::GraphAndConstant(result, graph_z.constant); }; - sum = sum.apply(add); + return sum.apply(add); } - return sum; } /* ************************************************************************ */ +// TODO(dellaert): At the time I though "Sum" was a good name, but coming back +// to it after a while I think "SumFrontals" is better.it's a terrible name. +// Also, the implementation is inconsistent. I think we should just have a +// virtual method in HybridFactor that adds to the "Sum" object, like +// addGaussian. Finally, we need to document why deferredFactors need to be +// added last, which I would undo if possible. +// Implementation-wise, it's probably more efficient to first collect the +// discrete keys, and then loop over all assignments to populate a vector. GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const { // sum out frontals, this is the factor on the separator gttic(sum); @@ -89,8 +96,8 @@ GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const { std::vector deferredFactors; for (auto &f : factors_) { + // TODO(dellaert): just use a virtual method defined in HybridFactor. if (f->isHybrid()) { - // TODO(dellaert): just use a virtual method defined in HybridFactor. if (auto gm = boost::dynamic_pointer_cast(f)) { sum = gm->add(sum); } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index db0687642..ed3ada2ff 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -18,10 +18,10 @@ #pragma once +#include #include #include #include -#include #include #include #include @@ -122,9 +122,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Adding factors. /// @{ - using Base::reserve; using Base::add; using Base::push_back; + using Base::reserve; /// Add a Jacobian factor to the factor graph. void add(JacobianFactor&& factor); @@ -236,8 +236,15 @@ class GTSAM_EXPORT HybridGaussianFactorGraph */ const Ordering getHybridOrdering() const; - /// Compute a DecisionTree with the marginal for - /// each discrete assignment. + /** + * @brief Create a decision tree of factor graphs out of this hybrid factor + * graph. + * + * For example, if there are two mixture factors, one with a discrete key A + * and one with a discrete key B, then the decision tree will have two levels, + * one for A and one for B. The leaves of the tree will be the Gaussian + * factors that have only continuous keys. + */ GaussianMixtureFactor::Sum SumFrontals() const; /// @}