From be86b9b5d74f906ccbedd9faef7b408a52861d32 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 22 Jan 2021 21:04:28 -0500 Subject: [PATCH 1/7] changed barcsq to be a vector, such that the user can provide a bound for each factor --- gtsam/nonlinear/GncOptimizer.h | 71 ++++++++++++++++++++++++++-------- gtsam/nonlinear/GncParams.h | 13 ------- tests/testGncOptimizer.cpp | 68 +++++++++++++++++++++++++++++--- 3 files changed, 118 insertions(+), 34 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index cd0b4c81a4..b7dc245d2c 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -43,6 +43,7 @@ class GncOptimizer { Values state_; ///< Initial values to be used at each iteration by GNC. GncParameters params_; ///< GNC parameters. Vector weights_; ///< Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside). + Vector barcSq_; ///< Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance. Also note the code allows a threshold for each factor. public: /// Constructor. @@ -53,6 +54,7 @@ class GncOptimizer { // make sure all noiseModels are Gaussian or convert to Gaussian nfg_.resize(graph.size()); + barcSq_ = Vector::Ones(graph.size()); for (size_t i = 0; i < graph.size(); i++) { if (graph[i]) { NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< @@ -65,6 +67,26 @@ class GncOptimizer { } } + /** Set the maximum weighted residual error for an inlier (same for all factors). 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 setInlierCostThresholds(const double inth) { + barcSq_ = inth * Vector::Ones(nfg_.size()); + } + + /** Set the maximum weighted residual error for an inlier (one for each factor). 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 setInlierCostThresholds(const Vector& inthVec) { + barcSq_ = inthVec; + } + /// Access a copy of the internal factor graph. const NonlinearFactorGraph& getFactors() const { return nfg_; } @@ -77,6 +99,17 @@ class GncOptimizer { /// Access a copy of the GNC weights. const Vector& getWeights() const { return weights_;} + /// Get the inlier threshold. + const Vector& getInlierCostThresholds() const {return barcSq_;} + + /// Equals. + bool equals(const GncOptimizer& other, double tol = 1e-9) const { + return nfg_.equals(other.getFactors()) + && equal(weights_, other.getWeights()) + && params_.equals(other.getParams()) + && equal(barcSq_, other.getInlierCostThresholds()); + } + /// Compute optimal solution using graduated non-convexity. Values optimize() { // start by assuming all measurements are inliers @@ -153,18 +186,18 @@ class GncOptimizer { /// Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper). double initializeMu() const { - // compute largest error across all factors - double rmax_sq = 0.0; - for (size_t i = 0; i < nfg_.size(); i++) { - if (nfg_[i]) { - rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_)); - } - } + + double mu_init = 0.0; // set initial mu switch (params_.lossType) { case GncLossType::GM: - // surrogate cost is convex for large mu - return 2 * rmax_sq / params_.barcSq; // initial mu + // surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper + for (size_t k = 0; k < nfg_.size(); k++) { + if (nfg_[k]) { + mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]); + } + } + return mu_init; // initial mu case GncLossType::TLS: /* initialize mu to the value specified in Remark 5 in GNC paper. surrogate cost is convex for mu close to zero @@ -172,9 +205,15 @@ class GncOptimizer { according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop */ - return - (2 * rmax_sq - params_.barcSq) > 0 ? - params_.barcSq / (2 * rmax_sq - params_.barcSq) : -1; + mu_init = std::numeric_limits::infinity(); + for (size_t k = 0; k < nfg_.size(); k++) { + if (nfg_[k]) { + double rk = nfg_[k]->error(state_); + mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise keep same + std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; + } + } + return mu_init > 0 && !isinf(mu_init) ? mu_init : -1; default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type."); @@ -305,14 +344,14 @@ class GncOptimizer { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual weights[k] = std::pow( - (mu * params_.barcSq) / (u2_k + mu * params_.barcSq), 2); + (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2); } } return weights; } case GncLossType::TLS: { // use eq (14) in GNC paper - double upperbound = (mu + 1) / mu * params_.barcSq; - double lowerbound = mu / (mu + 1) * params_.barcSq; + double upperbound = (mu + 1) / mu * barcSq_.maxCoeff(); + double lowerbound = mu / (mu + 1) * barcSq_.minCoeff(); for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual @@ -321,7 +360,7 @@ class GncOptimizer { } else if (u2_k <= lowerbound) { weights[k] = 1; } else { - weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k) + weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; } } diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 0388a7fd16..904d7b4349 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -66,7 +66,6 @@ class GncParams { /// any other specific GNC parameters: GncLossType lossType = TLS; ///< Default loss size_t maxIterations = 100; ///< Maximum number of iterations - double barcSq = 1.0; ///< A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance double muStep = 1.4; ///< Multiplicative factor to reduce/increase the mu in gnc double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) @@ -86,16 +85,6 @@ class GncParams { maxIterations = maxIter; } - /** 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 setInlierCostThreshold(const double inth) { - barcSq = inth; - } - /// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep. void setMuStep(const double step) { muStep = step; @@ -131,7 +120,6 @@ class GncParams { bool equals(const GncParams& other, double tol = 1e-9) const { return baseOptimizerParams.equals(other.baseOptimizerParams) && lossType == other.lossType && maxIterations == other.maxIterations - && std::fabs(barcSq - other.barcSq) <= tol && std::fabs(muStep - other.muStep) <= tol && verbosity == other.verbosity && knownInliers == other.knownInliers; } @@ -150,7 +138,6 @@ class GncParams { throw std::runtime_error("GncParams::print: unknown loss type."); } std::cout << "maxIterations: " << maxIterations << "\n"; - std::cout << "barcSq: " << barcSq << "\n"; std::cout << "muStep: " << muStep << "\n"; std::cout << "relativeCostTol: " << relativeCostTol << "\n"; std::cout << "weightsTol: " << weightsTol << "\n"; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 738c77936a..f49d600df1 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -87,6 +87,12 @@ TEST(GncOptimizer, gncConstructor) { CHECK(gnc.getFactors().equals(fg)); CHECK(gnc.getState().equals(initial)); CHECK(gnc.getParams().equals(gncParams)); + + auto gnc2 = GncOptimizer>(fg, initial, + gncParams); + + // check the equal works + CHECK(gnc.equals(gnc2)); } /* ************************************************************************* */ @@ -340,9 +346,10 @@ TEST(GncOptimizer, calculateWeightsGM) { mu = 2.0; double barcSq = 5.0; weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 - gncParams.setInlierCostThreshold(barcSq); + auto gnc2 = GncOptimizer>(fg, initial, gncParams); + gnc2.setInlierCostThresholds(barcSq); weights_actual = gnc2.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); } @@ -398,9 +405,10 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType(GncLossType::TLS); - gncParams.setInlierCostThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier auto gnc = GncOptimizer>(nfg, initial, gncParams); + gnc.setInlierCostThresholds(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier + double mu = 1e6; Vector weights_actual = gnc.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); @@ -414,9 +422,9 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType(GncLossType::TLS); - gncParams.setInlierCostThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier auto gnc = GncOptimizer>(nfg, initial, gncParams); + gnc.setInlierCostThresholds(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier double mu = 1e6; // very large mu recovers original TLS cost Vector weights_actual = gnc.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); @@ -430,9 +438,9 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType(GncLossType::TLS); - gncParams.setInlierCostThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier auto gnc = GncOptimizer>(nfg, initial, gncParams); + gnc.setInlierCostThresholds(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier double mu = 1e6; // very large mu recovers original TLS cost Vector weights_actual = gnc.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, 1e-5)); @@ -576,9 +584,9 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { gncParams.setKnownInliers(knownInliers); gncParams.setLossType(GncLossType::TLS); //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); - gncParams.setInlierCostThreshold(100.0); auto gnc = GncOptimizer>(fg, initial, gncParams); + gnc.setInlierCostThresholds(100.0); Values gnc_result = gnc.optimize(); CHECK(assert_equal(Point2(0.25, 0.0), gnc_result.at(X(1)), 1e-3)); @@ -592,6 +600,56 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { } } +/* ************************************************************************* */ +TEST(GncOptimizer, setWeights) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + + std::vector knownInliers; + knownInliers.push_back(0); + knownInliers.push_back(1); + knownInliers.push_back(2); + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setInlierCostThresholds(2.0); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + CHECK(assert_equal(2.0 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3)); + } + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setInlierCostThresholds(2.0 * Vector::Ones(fg.size())); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + CHECK(assert_equal(2.0 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3)); + } +} + /* ************************************************************************* */ TEST(GncOptimizer, optimizeSmallPoseGraph) { /// load small pose graph From fdced87b9a1b017febe12f7d8bc5c60520509d1a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 22 Jan 2021 22:22:54 -0500 Subject: [PATCH 2/7] trying to include chi2 --- gtsam/nonlinear/GncOptimizer.h | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index b7dc245d2c..f4b994126b 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -29,7 +29,18 @@ #include #include +//#include +#include + namespace gtsam { +/* + * Quantile of chi-squared distribution with given degrees of freedom at probability alpha. + * Equivalent to chi2inv in Matlab. + */ +static double Chi2inv(const double alpha, const size_t dofs) { + boost::math::chi_squared_distribution chi2(dofs); + return boost::math::quantile(chi2, alpha); +} /* ************************************************************************* */ template @@ -55,6 +66,16 @@ class GncOptimizer { // make sure all noiseModels are Gaussian or convert to Gaussian 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++) { if (graph[i]) { NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< @@ -63,6 +84,7 @@ class GncOptimizer { noiseModel::Robust>(factor->noiseModel()); // if the factor has a robust loss, we remove the robust loss nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; + barcSq_[i] = 0.5 * Chi2inv(alpha, nfg_[i]->dim()); // 0.5 derives from the error definition in gtsam } } } From 28b0f0ac8ecf8b5a67e18dde68c163fba7d73002 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 22 Jan 2021 22:27:47 -0500 Subject: [PATCH 3/7] working unit tests: added chi2 --- gtsam/nonlinear/GncOptimizer.h | 7 ------- tests/testGncOptimizer.cpp | 5 ++++- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index f4b994126b..c246c4f702 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 f49d600df1..50e3c0b73f 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)); From a59a12245c1423ecd5200c43aa458017ae4e4a82 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 22 Jan 2021 23:24:28 -0500 Subject: [PATCH 4/7] done with new default noise thresholds! --- gtsam/nonlinear/GncOptimizer.h | 21 ++++++++++--- tests/testGncOptimizer.cpp | 56 ++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index c246c4f702..dab2fb7be3 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -65,10 +65,6 @@ class GncOptimizer { // make sure all noiseModels are Gaussian or convert to Gaussian nfg_.resize(graph.size()); - barcSq_ = Vector::Ones(graph.size()); - - double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_ - for (size_t i = 0; i < graph.size(); i++) { if (graph[i]) { NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< @@ -77,9 +73,12 @@ class GncOptimizer { noiseModel::Robust>(factor->noiseModel()); // if the factor has a robust loss, we remove the robust loss nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; - barcSq_[i] = 0.5 * Chi2inv(alpha, nfg_[i]->dim()); // 0.5 derives from the error definition in gtsam } } + + // set default barcSq_ (inlier threshold) + double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_ + setInlierCostThresholdsAtProbability(alpha); } /** Set the maximum weighted residual error for an inlier (same for all factors). For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega, @@ -102,6 +101,18 @@ class GncOptimizer { barcSq_ = inthVec; } + /** Set the maximum weighted residual error threshold by specifying the probability + * alpha that the inlier residuals are smaller than that threshold + * */ + void setInlierCostThresholdsAtProbability(const double alpha) { + barcSq_ = Vector::Ones(nfg_.size()); + for (size_t k = 0; k < nfg_.size(); k++) { + if (nfg_[k]) { + barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam + } + } + } + /// Access a copy of the internal factor graph. const NonlinearFactorGraph& getFactors() const { return nfg_; } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 50e3c0b73f..981e475ca1 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -33,6 +33,9 @@ #include #include +#include +#include + using namespace std; using namespace gtsam; @@ -603,6 +606,59 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { } } +/* ************************************************************************* */ +TEST(GncOptimizer, chi2inv) { + DOUBLES_EQUAL(8.807468393511950, Chi2inv(0.997, 1), tol); // from MATLAB: chi2inv(0.997, 1) = 8.807468393511950 + DOUBLES_EQUAL(13.931422665512077, Chi2inv(0.997, 3), tol); // from MATLAB: chi2inv(0.997, 3) = 13.931422665512077 +} + +/* ************************************************************************* */ +TEST(GncOptimizer, barcsq) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + + std::vector knownInliers; + knownInliers.push_back(0); + knownInliers.push_back(1); + knownInliers.push_back(2); + + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + // expected: chi2inv(0.99, 2)/2 + CHECK(assert_equal(4.605170185988091 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, barcsq_heterogeneousFactors) { + NonlinearFactorGraph fg; + // specify noise model, otherwise it segfault if we leave default noise model + SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); + fg.add( PriorFactor( 0, Pose2(0.0, 0.0, 0.0) , model3D )); // size 3 + SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); + fg.add( PriorFactor( 1, Point2(0.0,0.0), model2D )); // size 2 + SharedNoiseModel model1D(noiseModel::Isotropic::Sigma(1, 0.5)); + fg.add( BearingFactor( 0, 1, 1.0, model1D) ); // size 1 + + Values initial; + initial.insert(0, Pose2(0.0, 0.0, 0.0)); + initial.insert(1, Point2(0.0,0.0)); + + auto gnc = GncOptimizer>(fg, initial); + CHECK(assert_equal(Vector3(5.672433365072185, 4.605170185988091, 3.317448300510607), + gnc.getInlierCostThresholds(), 1e-3)); + + // extra test: + // fg.add( PriorFactor( 0, Pose2(0.0, 0.0, 0.0) )); // works if we add model3D as noise model + // std::cout << "fg[3]->dim() " << fg[3]->dim() << std::endl; // this segfaults? +} + /* ************************************************************************* */ TEST(GncOptimizer, setWeights) { auto fg = example::sharedNonRobustFactorGraphWithOutliers(); From bd0a735ee848e91e8dd72e071078d85f6d990938 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 29 Jan 2021 19:05:29 -0500 Subject: [PATCH 5/7] improved comments --- gtsam/nonlinear/GncOptimizer.h | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index dab2fb7be3..ebdae765dc 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -54,7 +54,7 @@ class GncOptimizer { Values state_; ///< Initial values to be used at each iteration by GNC. GncParameters params_; ///< GNC parameters. Vector weights_; ///< Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside). - Vector barcSq_; ///< Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance. Also note the code allows a threshold for each factor. + Vector barcSq_; ///< Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq_[i] (where i is the position of the factor in the factor graph. Note that factor.error() whitens by the covariance. public: /// Constructor. @@ -84,7 +84,7 @@ class GncOptimizer { /** Set the maximum weighted residual error for an inlier (same for all factors). 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. + * Assuming an 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 setInlierCostThresholds(const double inth) { @@ -94,8 +94,6 @@ class GncOptimizer { /** Set the maximum weighted residual error for an inlier (one for each factor). 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 setInlierCostThresholds(const Vector& inthVec) { barcSq_ = inthVec; @@ -105,7 +103,7 @@ class GncOptimizer { * alpha that the inlier residuals are smaller than that threshold * */ void setInlierCostThresholdsAtProbability(const double alpha) { - barcSq_ = Vector::Ones(nfg_.size()); + barcSq_ = Vector::Ones(nfg_.size()); // initialize for (size_t k = 0; k < nfg_.size(); k++) { if (nfg_[k]) { barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam @@ -214,10 +212,12 @@ class GncOptimizer { double initializeMu() const { double mu_init = 0.0; - // set initial mu + // initialize mu to the value specified in Remark 5 in GNC paper. switch (params_.lossType) { case GncLossType::GM: - // surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper + /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper. + Since barcSq_ can be different for each factor, we compute the max of the quantity in remark 5 in GNC paper + */ for (size_t k = 0; k < nfg_.size(); k++) { if (nfg_[k]) { mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]); @@ -225,11 +225,11 @@ class GncOptimizer { } return mu_init; // initial mu case GncLossType::TLS: - /* initialize mu to the value specified in Remark 5 in GNC paper. - surrogate cost is convex for mu close to zero + /* surrogate cost is convex for mu close to zero. initialize as in remark 5 in GNC paper. degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop) according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual - however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop + however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop. + Since barcSq_ can be different for each factor, we look for the minimimum (positive) quantity in remark 5 in GNC paper */ mu_init = std::numeric_limits::infinity(); for (size_t k = 0; k < nfg_.size(); k++) { @@ -239,7 +239,9 @@ class GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - return mu_init > 0 && !isinf(mu_init) ? mu_init : -1; + return mu_init > 0 && !isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, + // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold + // and there is no need to robustify (TLS = least squares) default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type."); From 95e685296e695fdc66aefb94771168ba38f3145a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 30 Jan 2021 17:04:20 -0500 Subject: [PATCH 6/7] removed commented line --- gtsam/nonlinear/GncOptimizer.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index ebdae765dc..bedbc85d97 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -28,8 +28,6 @@ #include #include - -//#include #include namespace gtsam { From 566e4c4a0a2d6c4336c70ae8b67d1a59c65ec273 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Sun, 31 Jan 2021 17:41:28 -0500 Subject: [PATCH 7/7] use std namespace qualifier --- gtsam/nonlinear/GncOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index bedbc85d97..d319fd5fd2 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -237,7 +237,7 @@ class GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - return mu_init > 0 && !isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, + return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) default: