diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 6a9fc5c35..b04db4977 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -212,16 +212,15 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() } /* *******************************************************************************/ -double HybridGaussianFactor::potentiallyPrunedComponentError( - const sharedFactor &gf, const VectorValues &values) const { +/// Helper method to compute the error of a component. +static double PotentiallyPrunedComponentError( + const GaussianFactor::shared_ptr &gf, const VectorValues &values) { // Check if valid pointer if (gf) { return gf->error(values); } else { - // If not valid, pointer, it means this component was pruned, - // so we return maximum error. - // This way the negative exponential will give - // a probability value close to 0.0. + // If nullptr this component was pruned, so we return maximum error. This + // way the negative exponential will give a probability value close to 0.0. return std::numeric_limits::max(); } } @@ -230,8 +229,8 @@ double HybridGaussianFactor::potentiallyPrunedComponentError( AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [this, &continuousValues](const sharedFactor &gf) { - return this->potentiallyPrunedComponentError(gf, continuousValues); + auto errorFunc = [&continuousValues](const sharedFactor &gf) { + return PotentiallyPrunedComponentError(gf, continuousValues); }; DecisionTree error_tree(factors_, errorFunc); return error_tree; @@ -241,7 +240,7 @@ AlgebraicDecisionTree HybridGaussianFactor::errorTree( double HybridGaussianFactor::error(const HybridValues &values) const { // Directly index to get the component, no need to build the whole tree. const sharedFactor gf = factors_(values.discrete()); - return potentiallyPrunedComponentError(gf, values.continuous()); + return PotentiallyPrunedComponentError(gf, values.continuous()); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index f23d065b6..e5a575409 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -189,10 +189,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { */ static Factors augment(const FactorValuePairs &factors); - /// Helper method to compute the error of a component. - double potentiallyPrunedComponentError( - const sharedFactor &gf, const VectorValues &continuousValues) const; - /// Helper struct to assist private constructor below. struct ConstructorHelper;