diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index f4b994126..c246c4f70 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -67,13 +67,6 @@ class GncOptimizer { nfg_.resize(graph.size()); barcSq_ = Vector::Ones(graph.size()); - boost::math::chi_squared_distribution chi2inv(3.0); - - std::cout << "chi2inv.degrees_of_freedom() = " << chi2inv.degrees_of_freedom() << std::endl; - - double a = boost::math::quantile(chi2inv, 0.997); - std::cout << " a " << a << std::endl; - double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_ for (size_t i = 0; i < graph.size(); i++) { diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index f49d600df..50e3c0b73 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -128,6 +128,7 @@ TEST(GncOptimizer, initializeMu) { gncParams.setLossType(GncLossType::GM); auto gnc_gm = GncOptimizer>(fg, initial, gncParams); + gnc_gm.setInlierCostThresholds(1.0); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq // (barcSq=1 in this example) EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3); @@ -136,6 +137,7 @@ TEST(GncOptimizer, initializeMu) { gncParams.setLossType(GncLossType::TLS); auto gnc_tls = GncOptimizer>(fg, initial, gncParams); + gnc_tls.setInlierCostThresholds(1.0); // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) // (barcSq=1 in this example) EXPECT_DOUBLES_EQUAL(gnc_tls.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3); @@ -339,6 +341,7 @@ TEST(GncOptimizer, calculateWeightsGM) { GncParams gncParams(gnParams); gncParams.setLossType(GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); + gnc.setInlierCostThresholds(1.0); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); @@ -550,7 +553,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); - + gnc.setInlierCostThresholds(1.0); Values gnc_result = gnc.optimize(); CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3));