diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 6d19ef156d..0e7e9c692d 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -179,4 +179,35 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( return linearFG; } +/* ************************************************************************* */ +AlgebraicDecisionTree HybridNonlinearFactorGraph::errorTree( + const Values& values) const { + AlgebraicDecisionTree result(0.0); + + // Iterate over each factor. + for (auto& factor : factors_) { + if (auto hnf = std::dynamic_pointer_cast(factor)) { + // Compute factor error and add it. + result = result + hnf->errorTree(values); + + } else if (auto nf = std::dynamic_pointer_cast(factor)) { + // If continuous only, get the (double) error + // and add it to every leaf of the result + result = result + nf->error(values); + + } else if (auto df = std::dynamic_pointer_cast(factor)) { + // If discrete, just add its errorTree as well + result = result + df->errorTree(); + + } else { + throw std::runtime_error( + "HybridNonlinearFactorGraph::errorTree(Values) not implemented for " + "factor type " + + demangle(typeid(factor).name()) + "."); + } + } + + return result; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 5a09f18d45..53920a4aad 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -90,6 +90,19 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { /// Expose error(const HybridValues&) method. using Base::error; + /** + * @brief Compute error of (hybrid) nonlinear factors and discrete factors + * over each discrete assignment, and return as a tree. + * + * Error \f$ e = \Vert f(x) - \mu \Vert_{\Sigma} \f$. + * + * @note: Gaussian and hybrid Gaussian factors are not considered! + * + * @param values Manifold values at which to compute the error. + * @return AlgebraicDecisionTree + */ + AlgebraicDecisionTree errorTree(const Values& values) const; + /// @} }; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 4735c16573..647a8b6462 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -211,6 +211,37 @@ TEST(HybridNonlinearFactorGraph, PushBack) { // EXPECT_LONGS_EQUAL(3, hnfg.size()); } +/* ****************************************************************************/ +// Test hybrid nonlinear factor graph errorTree +TEST(HybridNonlinearFactorGraph, ErrorTree) { + Switching s(3); + + HybridNonlinearFactorGraph graph = s.nonlinearFactorGraph; + Values values = s.linearizationPoint; + + auto error_tree = graph.errorTree(s.linearizationPoint); + + auto dkeys = graph.discreteKeys(); + DiscreteKeys discrete_keys(dkeys.begin(), dkeys.end()); + + // Compute the sum of errors for each factor. + auto assignments = DiscreteValues::CartesianProduct(discrete_keys); + std::vector leaves(assignments.size()); + for (auto &&factor : graph) { + for (size_t i = 0; i < assignments.size(); ++i) { + leaves[i] += + factor->error(HybridValues(VectorValues(), assignments[i], values)); + } + } + // Swap i=1 and i=2 to give correct ordering. + double temp = leaves[1]; + leaves[1] = leaves[2]; + leaves[2] = temp; + AlgebraicDecisionTree expected_error(discrete_keys, leaves); + + EXPECT(assert_equal(expected_error, error_tree, 1e-7)); +} + /**************************************************************************** * Test construction of switching-like hybrid factor graph. */