Implement errorTree for HybridNonlinearFactorGraph

gtsam/hybrid/HybridNonlinearFactorGraph.cpp

@@ -179,4 +179,35 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
   return linearFG;
 }
 
+/* ************************************************************************* */
+AlgebraicDecisionTree<Key> HybridNonlinearFactorGraph::errorTree(
+    const Values& values) const {
+  AlgebraicDecisionTree<Key> result(0.0);
+
+  // Iterate over each factor.
+  for (auto& factor : factors_) {
+    if (auto hnf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
+      // Compute factor error and add it.
+      result = result + hnf->errorTree(values);
+
+    } else if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(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<DiscreteFactor>(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

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<Key>
+   */
+  AlgebraicDecisionTree<Key> errorTree(const Values& values) const;
+
   /// @}
 };
 

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<double> 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<Key> expected_error(discrete_keys, leaves);
+
+  EXPECT(assert_equal(expected_error, error_tree, 1e-7));
+}
+
 /****************************************************************************
  * Test construction of switching-like hybrid factor graph.
  */