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,24 @@ 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;
+
+  auto error_tree = graph.errorTree(s.linearizationPoint);
+
+  auto dkeys = graph.discreteKeys();
+  std::vector<DiscreteKey> discrete_keys(dkeys.begin(), dkeys.end());
+  std::vector<double> leaves = {152.79175946923, 151.59861228867,
+                                151.70397280433, 151.60943791243};
+  AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
+  // regression
+  EXPECT(assert_equal(expected_error, error_tree, 1e-7));
+}
+
 /****************************************************************************
  * Test construction of switching-like hybrid factor graph.
  */