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

Added a unit test for EssentialMatrixFactor3 with LevenbergMarquardt optimization

release/4.3a0
Frank Dellaert 2014-01-04 11:59:57 -05:00
parent f8fbfaea50
commit 651dd3e931
1 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) {
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, LieScalar(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
} }
} // namespace example1 } // namespace example1