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

add unit test

release/4.3a0
Varun Agrawal 2024-10-01 14:38:51 -04:00
parent d2f1894bac
commit 2463245ec2
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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.
*/