From cd82a56214b931884e83e19a0182252add9b7687 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 21 Dec 2020 22:32:34 -0500 Subject: [PATCH] made function name less ambiguous, added more comments on inlierThreshold --- gtsam/nonlinear/GncOptimizer.h | 5 ++++- tests/testGncOptimizer.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d4b25409f..df87c4433 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -83,8 +83,11 @@ public: } /** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega, * the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier. + * In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq. + * Assuming a isotropic measurement covariance sigma^2 * Identity, the cost becomes: 0.5 * 1/sigma^2 || r(x) ||^2 <= barcSq. + * Hence || r(x) ||^2 <= 2 * barcSq * sigma^2 * */ - void setInlierThreshold(const double inth) { + void setInlierCostThreshold(const double inth) { barcSq = inth; } /// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 85a196bd7..7c4fbd5fa 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -254,7 +254,7 @@ TEST(GncOptimizer, calculateWeightsGM) { double barcSq = 5.0; weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 - gncParams.setInlierThreshold(barcSq); + gncParams.setInlierCostThreshold(barcSq); auto gnc2 = GncOptimizer>(fg, initial, gncParams); weights_actual = gnc2.calculateWeights(initial, mu); @@ -315,7 +315,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GncParams gncParams(gnParams); gncParams.setLossType( GncParams::RobustLossType::TLS); - gncParams.setInlierThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier + gncParams.setInlierCostThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier auto gnc = GncOptimizer>(nfg, initial, gncParams); double mu = 1e6; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -331,7 +331,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GncParams gncParams(gnParams); gncParams.setLossType( GncParams::RobustLossType::TLS); - gncParams.setInlierThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier + gncParams.setInlierCostThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier auto gnc = GncOptimizer>(nfg, initial, gncParams); double mu = 1e6; // very large mu recovers original TLS cost Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -347,7 +347,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GncParams gncParams(gnParams); gncParams.setLossType( GncParams::RobustLossType::TLS); - gncParams.setInlierThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier + gncParams.setInlierCostThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier auto gnc = GncOptimizer>(nfg, initial, gncParams); double mu = 1e6; // very large mu recovers original TLS cost Vector weights_actual = gnc.calculateWeights(initial, mu);