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

more unit tests

release/4.3a0
jingnanshi 2020-12-07 20:20:51 -05:00
parent 594f63d1f6
commit 398c01375e
1 changed files with 28 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
tests/testGncOptimizer.cpp
View File

@ -225,7 +225,7 @@ TEST(GncOptimizer, checkMuConvergenceTLS) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GncOptimizer, calculateWeights) { TEST(GncOptimizer, calculateWeightsGM) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers(); auto fg = example::sharedNonRobustFactorGraphWithOutliers();
Point2 p0(0, 0); Point2 p0(0, 0);
@ -242,6 +242,8 @@ TEST(GncOptimizer, calculateWeights) {
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(
GncParams<GaussNewtonParams>::RobustLossType::GM);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams); auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
double mu = 1.0; double mu = 1.0;
Vector weights_actual = gnc.calculateWeights(initial, mu); Vector weights_actual = gnc.calculateWeights(initial, mu);
@ -258,6 +260,31 @@ TEST(GncOptimizer, calculateWeights) {
CHECK(assert_equal(weights_expected, weights_actual, tol)); CHECK(assert_equal(weights_expected, weights_actual, tol));
} }
/* ************************************************************************* */
TEST(GncOptimizer, calculateWeightsTLS) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
Point2 p0(0, 0);
Values initial;
initial.insert(X(1), p0);
// we have 4 factors, 3 with zero errors (inliers), 1 with error
Vector weights_expected = Vector::Zero(4);
weights_expected[0] = 1.0; // zero error
weights_expected[1] = 1.0; // zero error
weights_expected[2] = 1.0; // zero error
weights_expected[3] = 0; // outliers
GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(
GncParams<GaussNewtonParams>::RobustLossType::TLS);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
double mu = 1.0;
Vector weights_actual = gnc.calculateWeights(initial, mu);
CHECK(assert_equal(weights_expected, weights_actual, tol));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GncOptimizer, makeWeightedGraph) { TEST(GncOptimizer, makeWeightedGraph) {
// create original factor // create original factor