From dc5c769e7ca4a9cd6d23a1def4d4a95bfa32a93e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 23 Dec 2020 22:08:44 -0500 Subject: [PATCH] - fixed stopping conditions - handled degenerate case in mu initialization - set TLS as default - added more unit tests --- gtsam/nonlinear/GncOptimizer.h | 128 +++++++++++++++++---------- tests/testGncOptimizer.cpp | 155 ++++++++++++++++++++++++++++++++- 2 files changed, 233 insertions(+), 50 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d47b6bc82..b66e0f523 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -62,11 +62,12 @@ public: /// GNC parameters BaseOptimizerParameters baseOptimizerParams; /*optimization parameters used to solve the weighted least squares problem at each GNC iteration*/ /// any other specific GNC parameters: - RobustLossType lossType = GM; /* default loss*/ + RobustLossType 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; ///< The maximum relative cost change to stop iterating + double relativeCostTol = 1e-5; ///< if relative cost change if below this threshold, stop iterating + double weightsTol = 1e-4; ///< if the weights are within weightsTol from being binary, stop iterating (only for TLS) VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ @@ -97,6 +98,9 @@ public: /// Set the maximum relative difference in mu values to stop iterating void setRelativeCostTol(double value) { relativeCostTol = value; } + /// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating + void setWeightsTol(double value) { weightsTol = value; + } /// Set the verbosity level void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; @@ -136,6 +140,8 @@ public: 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"; std::cout << "verbosityGNC: " << verbosityGNC << "\n"; for (size_t i = 0; i < knownInliers.size(); i++) std::cout << "knownInliers: " << knownInliers[i] << "\n"; @@ -205,8 +211,8 @@ public: BaseOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); - double cost = calculateWeightedCost(); - double prev_cost = cost; + double prev_cost = nfg_.error(result); + double cost = 0.0; // this will be updated in the main loop // handle the degenerate case that corresponds to small // maximum residual errors at initialization @@ -215,16 +221,21 @@ public: if (mu <= 0) { if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " - "initialization is small." << std::endl; + "initialization is small." << std::endl; + } + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { result.print("result\n"); + std::cout << "mu: " << mu << std::endl; } return result; } - for (size_t iter = 0; iter < params_.maxIterations; iter++) { + size_t iter; + for (iter = 0; iter < params_.maxIterations; iter++) { // display info if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + std::cout << "iter: " << iter << std::endl; result.print("result\n"); std::cout << "mu: " << mu << std::endl; std::cout << "weights: " << weights_ << std::endl; @@ -232,28 +243,34 @@ public: // weights update weights_ = calculateWeights(result, mu); - // update cost - prev_cost = cost; - cost = calculateWeightedCost(); - // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); BaseOptimizer baseOptimizer_iter(graph_iter, state_); result = baseOptimizer_iter.optimize(); // stopping condition - if (checkConvergence(mu, cost, prev_cost)) { - // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { - std::cout << "final iterations: " << iter << std::endl; - std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; - } - break; - } + cost = graph_iter.error(result); + if (checkConvergence(mu, weights_, cost, prev_cost)) { break; } // update mu mu = updateMu(mu); + + // get ready for next iteration + prev_cost = cost; + + // display info + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; + } + } + // display info + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { + std::cout << "final iterations: " << iter << std::endl; + std::cout << "final mu: " << mu << std::endl; + std::cout << "final weights: " << weights_ << std::endl; + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; } return result; } @@ -276,7 +293,9 @@ public: // initialize mu to the value specified in Remark 5 in GNC paper. // surrogate cost is convex for mu close to zero // degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop) - return params_.barcSq / (2 * rmax_sq - params_.barcSq); + // 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; default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type."); @@ -298,53 +317,68 @@ public: } } - /// calculated sum of weighted squared residuals - double calculateWeightedCost() const { - double cost = 0; - for (size_t i = 0; i < nfg_.size(); i++) { - cost += weights_[i] * nfg_[i]->error(state_); - } - return cost; - } - /// check if we have reached the value of mu for which the surrogate loss matches the original loss bool checkMuConvergence(const double mu) const { + bool muConverged = false; switch (params_.lossType) { case GncParameters::GM: - return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + break; + case GncParameters::TLS: + muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity) + break; default: throw std::runtime_error( "GncOptimizer::checkMuConvergence: called with unknown loss type."); } + if (muConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + std::cout << "muConverged = true " << std::endl; + return muConverged; } /// check convergence of relative cost differences bool checkCostConvergence(const double cost, const double prev_cost) const { - switch (params_.lossType) { - case GncParameters::TLS: - return std::fabs(cost - prev_cost) < params_.relativeCostTol; - default: - throw std::runtime_error( - "GncOptimizer::checkMuConvergence: called with unknown loss type."); - } + bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost,1e-7) < params_.relativeCostTol; + if (costConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + std::cout << "checkCostConvergence = true " << std::endl; + return costConverged; } + /// check convergence of weights to binary values + bool checkWeightsConvergence(const Vector& weights) const { + bool weightsConverged = false; + switch (params_.lossType) { + case GncParameters::GM: + weightsConverged = false; // for GM, there is no clear binary convergence for the weights + break; + case GncParameters::TLS: + weightsConverged = true; + for(size_t i=0; i params_.weightsTol ){ + weightsConverged = false; + break; + } + } + break; + default: + throw std::runtime_error( + "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); + } + if (weightsConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + std::cout << "weightsConverged = true " << std::endl; + return weightsConverged; + } + /// check for convergence between consecutive GNC iterations bool checkConvergence(const double mu, + const Vector& weights, const double cost, const double prev_cost) const { - switch (params_.lossType) { - case GncParameters::GM: - return checkMuConvergence(mu); - case GncParameters::TLS: - return checkCostConvergence(cost, prev_cost); - default: - throw std::runtime_error( - "GncOptimizer::checkMuConvergence: called with unknown loss type."); - } + return checkCostConvergence(cost,prev_cost) || + checkWeightsConvergence(weights) || + checkMuConvergence(mu); } - /// create a graph where each factor is weighted by the gnc weights NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const { // make sure all noiseModels are Gaussian or convert to Gaussian diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index dc96762b8..9036fc97f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -66,7 +66,7 @@ TEST(GncOptimizer, gncParamsConstructor) { // change something at the gncParams level GncParams gncParams2c(gncParams2b); - gncParams2c.setLossType(GncParams::RobustLossType::TLS); + gncParams2c.setLossType(GncParams::RobustLossType::GM); CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); } @@ -186,7 +186,7 @@ TEST(GncOptimizer, updateMuTLS) { } /* ************************************************************************* */ -TEST(GncOptimizer, checkMuConvergenceGM) { +TEST(GncOptimizer, checkMuConvergence) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -194,6 +194,7 @@ TEST(GncOptimizer, checkMuConvergenceGM) { Values initial; initial.insert(X(1), p0); + { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); gncParams.setLossType( @@ -203,6 +204,112 @@ TEST(GncOptimizer, checkMuConvergenceGM) { double mu = 1.0; CHECK(gnc.checkMuConvergence(mu)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double mu = 1.0; + CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkCostConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setRelativeCostTol(0.49); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double prev_cost = 1.0; + double cost = 0.5; + // relative cost reduction = 0.5 > 0.49, hence checkCostConvergence = false + CHECK(!gnc.checkCostConvergence(cost, prev_cost)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setRelativeCostTol(0.51); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double prev_cost = 1.0; + double cost = 0.5; + // relative cost reduction = 0.5 < 0.51, hence checkCostConvergence = true + CHECK(gnc.checkCostConvergence(cost, prev_cost)); + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkWeightsConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + // weights are binary, so checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + weights[0] = 0.9; // more than weightsTol = 1e-4 from 1, hence checkWeightsConvergence = false + CHECK(!gnc.checkWeightsConvergence(weights)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + gncParams.setWeightsTol(0.1); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + weights[0] = 0.9; // exactly weightsTol = 0.1 from 1, hence checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); + } } /* ************************************************************************* */ @@ -455,9 +562,12 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { knownInliers.push_back(2); // nonconvexity with known inliers + { GncParams gncParams; gncParams.setKnownInliers(knownInliers); - // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + gncParams.setLossType( + GncParams::RobustLossType::GM); + //gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); Values gnc_result = gnc.optimize(); @@ -468,6 +578,45 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { DOUBLES_EQUAL(1.0, finalWeights[0], tol); DOUBLES_EQUAL(1.0, finalWeights[1], tol); DOUBLES_EQUAL(1.0, finalWeights[2], tol); + } + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + 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); + DOUBLES_EQUAL(0.0, finalWeights[3], tol); + } + { + // if we set the threshold large, they are all inliers + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + //gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + gncParams.setInlierCostThreshold( 100.0 ); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.25, 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); + DOUBLES_EQUAL(1.0, finalWeights[3], tol); + } } /* ************************************************************************* */