diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index e01df539e..44a56910e 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -31,45 +31,6 @@ namespace gtsam { -/* *******************************************************************************/ -HybridGaussianFactor::FactorValuePairs HybridGaussianFactor::augment( - const FactorValuePairs& factors) { - // Find the minimum value so we can "proselytize" to positive values. - // Done because we can't have sqrt of negative numbers. - DecisionTree gaussianFactors; - AlgebraicDecisionTree valueTree; - std::tie(gaussianFactors, valueTree) = unzip(factors); - - // Compute minimum value for normalization. - double min_value = valueTree.min(); - - // Finally, update the [A|b] matrices. - auto update = [&min_value](const auto& gfv) -> GaussianFactorValuePair { - auto [gf, value] = gfv; - - auto jf = std::dynamic_pointer_cast(gf); - if (!jf) return {gf, 0.0}; // should this be zero or infinite? - - double normalized_value = value - min_value; - - // If the value is 0, do nothing - if (normalized_value == 0.0) return {gf, value}; - - GaussianFactorGraph gfg; - gfg.push_back(jf); - - Vector c(1); - // When hiding c inside the `b` vector, value == 0.5*c^2 - c << std::sqrt(2.0 * normalized_value); - auto constantFactor = std::make_shared(c); - - gfg.push_back(constantFactor); - // NOTE(Frank): we store the actual value, not the normalized value: - return {std::make_shared(gfg), value}; - }; - return FactorValuePairs(factors, update); -} - /* *******************************************************************************/ struct HybridGaussianFactor::ConstructorHelper { KeyVector continuousKeys; // Continuous keys extracted from factors @@ -88,10 +49,10 @@ struct HybridGaussianFactor::ConstructorHelper { } } // Build the FactorValuePairs DecisionTree - pairs = FactorValuePairs(DecisionTree(discreteKeys, factors), - [](const auto& f) { - return std::pair{f, 0.0}; - }); + pairs = FactorValuePairs( + DecisionTree(discreteKeys, factors), [](const auto& f) { + return std::pair{f, f ? 0.0 : std::numeric_limits::infinity()}; + }); } /// Constructor for a single discrete key and a vector of GaussianFactorValuePairs @@ -128,7 +89,7 @@ struct HybridGaussianFactor::ConstructorHelper { /* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper& helper) - : Base(helper.continuousKeys, helper.discreteKeys), factors_(augment(helper.pairs)) {} + : Base(helper.continuousKeys, helper.discreteKeys), factors_(helper.pairs) {} HybridGaussianFactor::HybridGaussianFactor( const DiscreteKey& discreteKey, const std::vector& factorPairs) @@ -187,8 +148,7 @@ void HybridGaussianFactor::print(const std::string& s, const KeyFormatter& forma } /* *******************************************************************************/ -GaussianFactorValuePair HybridGaussianFactor::operator()( - const DiscreteValues& assignment) const { +GaussianFactorValuePair HybridGaussianFactor::operator()(const DiscreteValues& assignment) const { return factors_(assignment); } @@ -202,26 +162,13 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() 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 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(); - } -} - /* *******************************************************************************/ AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues& continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [&continuousValues](const auto& pair) { - return PotentiallyPrunedComponentError(pair.first, continuousValues); + return pair.first ? pair.first->error(continuousValues) + pair.second + : std::numeric_limits::infinity(); }; DecisionTree error_tree(factors_, errorFunc); return error_tree; @@ -231,7 +178,8 @@ AlgebraicDecisionTree HybridGaussianFactor::errorTree( double HybridGaussianFactor::error(const HybridValues& values) const { // Directly index to get the component, no need to build the whole tree. const auto pair = factors_(values.discrete()); - return PotentiallyPrunedComponentError(pair.first, values.continuous()); + return pair.first ? pair.first->error(values.continuous()) + pair.second + : std::numeric_limits::infinity(); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 95acb6ad6..c2d84fdb8 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -154,8 +154,6 @@ void HybridGaussianFactorGraph::printErrors( } /* ************************************************************************ */ -// TODO(dellaert): it's probably more efficient to first collect the discrete -// keys, and then loop over all assignments to populate a vector. HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor() const { HybridGaussianProductFactor result; @@ -270,6 +268,7 @@ static std::shared_ptr createDiscreteFactor(const ResultTree& eliminatio const DiscreteKeys& discreteSeparator) { auto potential = [&](const auto& pair) -> double { const auto& [conditional, factor] = pair.first; + const double scalar = pair.second; if (conditional && factor) { // If the factor is not null, it has no keys, just contains the residual. @@ -278,8 +277,8 @@ static std::shared_ptr createDiscreteFactor(const ResultTree& eliminatio // negLogConstant gives `-log(k)` // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`. const double negLogK = conditional->negLogConstant(); - const double old = factor->error(kEmpty) - negLogK; - return exp(-old); + const double error = scalar + factor->error(kEmpty) - negLogK; + return exp(-error); } else if (!conditional && !factor) { // If the factor is null, it has been pruned, hence return potential of zero return 0; @@ -302,17 +301,10 @@ static std::shared_ptr createHybridGaussianFactor(const ResultTree& elim const auto& [conditional, factor] = pair.first; const double scalar = pair.second; if (conditional && factor) { - auto hf = std::dynamic_pointer_cast(factor); - if (!hf) throw std::runtime_error("Expected HessianFactor!"); - // Add 2.0 term since the constant term will be premultiplied by 0.5 - // as per the Hessian definition, - // and negative since we want log(k) const double negLogK = conditional->negLogConstant(); - hf->constantTerm() += -2.0 * negLogK; - return {factor, negLogK}; - return {factor, scalar + negLogK}; + return {factor, scalar - negLogK}; } else if (!conditional && !factor) { - return {nullptr, 0.0}; // TODO(frank): or should this be infinity? + return {nullptr, std::numeric_limits::infinity()}; } else { throw std::runtime_error("createHybridGaussianFactors has mixed NULLs"); }