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

add HybridNonlinearFactorGraph::error test

release/4.3a0
Varun Agrawal 2024-09-20 15:30:06 -04:00
parent 9cbc7540d6
commit c71f0336e2
1 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -401,6 +401,37 @@ TEST(HybridNonlinearFactorGraph, Partial_Elimination) {
EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)}));
}
/* ****************************************************************************/
TEST(HybridNonlinearFactorGraph, Error) {
Switching self(3);
HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph;
{
HybridValues values(VectorValues(), DiscreteValues{{M(0), 0}, {M(1), 0}},
self.linearizationPoint);
// regression
EXPECT_DOUBLES_EQUAL(152.791759469, fg.error(values), 1e-9);
}
{
HybridValues values(VectorValues(), DiscreteValues{{M(0), 0}, {M(1), 1}},
self.linearizationPoint);
// regression
EXPECT_DOUBLES_EQUAL(151.598612289, fg.error(values), 1e-9);
}
{
HybridValues values(VectorValues(), DiscreteValues{{M(0), 1}, {M(1), 0}},
self.linearizationPoint);
// regression
EXPECT_DOUBLES_EQUAL(151.703972804, fg.error(values), 1e-9);
}
{
HybridValues values(VectorValues(), DiscreteValues{{M(0), 1}, {M(1), 1}},
self.linearizationPoint);
// regression
EXPECT_DOUBLES_EQUAL(151.609437912, fg.error(values), 1e-9);
}
}
/* ****************************************************************************/
TEST(HybridNonlinearFactorGraph, PrintErrors) {
Switching self(3);