From dc5c4cb99dbaf7361e70b33cfa4417886a00d40f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 1 Oct 2024 15:50:33 -0400
Subject: [PATCH] compute expected error

---
 .../tests/testHybridNonlinearFactorGraph.cpp  | 21 +++++++++++++++----
 1 file changed, 17 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 8221321ea5..647a8b6462 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -217,15 +217,28 @@ 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();
-  std::vector<DiscreteKey> discrete_keys(dkeys.begin(), dkeys.end());
-  std::vector<double> leaves = {152.79175946923, 151.59861228867,
-                                151.70397280433, 151.60943791243};
+  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);
-  // regression
+
   EXPECT(assert_equal(expected_error, error_tree, 1e-7));
 }