diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5734dfc43..b3bef11e0 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -225,7 +225,7 @@ TEST(GncOptimizer, checkMuConvergenceTLS) { } /* ************************************************************************* */ -TEST(GncOptimizer, calculateWeights) { +TEST(GncOptimizer, calculateWeightsGM) { auto fg = example::sharedNonRobustFactorGraphWithOutliers(); Point2 p0(0, 0); @@ -242,6 +242,8 @@ TEST(GncOptimizer, calculateWeights) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -258,6 +260,31 @@ TEST(GncOptimizer, calculateWeights) { 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 gncParams(gnParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = GncOptimizer>(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) { // create original factor