12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

compute expected error

release/4.3a0
Varun Agrawal 2024-10-01 15:50:33 -04:00
parent 2463245ec2
commit dc5c4cb99d
1 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -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));
}