From f897fa81a948dcf13e27c002096a44dee40dab14 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 15:14:41 -0500 Subject: [PATCH] added gnc loop --- tests/testGncOptimizer.cpp | 206 +++++++++++++++++++++++++++++++------ 1 file changed, 174 insertions(+), 32 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 1d3057335..87ac26841 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -29,21 +29,70 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::L; +static double tol = 1e-7; /* ************************************************************************* */ template class GncParams { public: - // using BaseOptimizer = BaseOptimizerParameters::OptimizerType; - GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {} + /** See NonlinearOptimizerParams::verbosity */ + enum RobustLossType { + GM /*Geman McClure*/, TLS /*Truncated least squares*/ + }; + + // using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; + + GncParams(const BaseOptimizerParameters& baseOptimizerParams): + baseOptimizerParams(baseOptimizerParams), + lossType(GM), /* default loss*/ + maxIterations(100), /* maximum number of iterations*/ + barcSq(1.0), /* a factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance*/ + muStep(1.4){}/* multiplicative factor to reduce/increase the mu in gnc */ // default constructor GncParams(): baseOptimizerParams() {} BaseOptimizerParameters baseOptimizerParams; - /// any other specific GNC parameters: + RobustLossType lossType; + size_t maxIterations; + double barcSq; + double muStep; + + void setLossType(RobustLossType type){ lossType = type; } + void setMaxIterations(size_t maxIter){ + std::cout + << "setMaxIterations: changing the max number of iterations might lead to less accurate solutions and is not recommended! " + << std::endl; + maxIterations = maxIter; + } + void setInlierThreshold(double inth){ barcSq = inth; } + void setMuStep(double step){ muStep = step; } + + /// equals + 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; + } + + /// print function + void print(const std::string& str) const { + std::cout << str << "\n"; + switch(lossType) { + case GM: std::cout << "lossType: Geman McClure" << "\n"; break; + default: + throw std::runtime_error( + "GncParams::print: unknown loss type."); + } + std::cout << "maxIterations: " << maxIterations << "\n"; + std::cout << "barcSq: " << barcSq << "\n"; + std::cout << "muStep: " << muStep << "\n"; + baseOptimizerParams.print(str); + } }; /* ************************************************************************* */ @@ -64,33 +113,88 @@ public: // TODO: Check that all noise models are Gaussian } -// Values optimize() const { -// NonlinearFactorGraph currentGraph = graph_; -// for (i : {1, 2, 3}) { -// BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial); -// VALUES currentSolution = baseOptimizer.optimize(); -// if (converged) { -// return currentSolution; -// } -// graph_i = this->makeGraph(currentSolution); -// } -//} + NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); } + Values getState() const { return Values(state_); } + GncParameters getParams() const { return GncParameters(params_); } -//NonlinearFactorGraph makeGraph(const Values& currentSolution) const { -// // calculate some weights -// this->calculateWeights(); -// // copy the graph with new weights -// -//} + /// implement GNC main loop, including graduating nonconvexity with mu + Values optimize() { + // start by assuming all measurements are inliers + Vector weights = Vector::Ones(nfg_.size()); + GaussNewtonOptimizer baseOptimizer(nfg_,state_); + Values result = baseOptimizer.optimize(); + double mu = initializeMu(); + for(size_t iter=0; iter < params_.maxIterations; iter++){ + // weights update + weights = calculateWeights(result, mu); + + // variable/values update + NonlinearFactorGraph graph_iter = this->makeGraph(weights); + GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); + Values result = baseOptimizer.optimize(); + + // stopping condition + if( checkMuConvergence(mu) ) { break; } + + // otherwise update mu + mu = updateMu(mu); + } + return result; + } + + /// initialize the gnc parameter mu such that loss is approximately convex + 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_)); + } + } + // set initial mu + switch(params_.lossType) { + case GncParameters::GM: + return 2*rmax_sq / params_.barcSq; // initial mu + default: + throw std::runtime_error( + "GncOptimizer::initializeMu: called with unknown loss type."); + } + } + + /// update the gnc parameter mu to gradually increase nonconvexity + double updateMu(const double mu) const { + switch(params_.lossType) { + case GncParameters::GM: + return std::max(1.0 , mu / params_.muStep); // reduce mu, but saturate at 1 + default: + throw std::runtime_error( + "GncOptimizer::updateMu: called with unknown loss type."); + } + } + + /// check if we have reached the value of mu for which the surrogate loss matches the original loss + bool checkMuConvergence(const double mu) const { + switch(params_.lossType) { + case GncParameters::GM: + return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + default: + throw std::runtime_error( + "GncOptimizer::checkMuConvergence: called with unknown loss type."); + } + } + + /// create a graph where each factor is weighted by the gnc weights + NonlinearFactorGraph makeGraph(const Vector& weights) const { + return NonlinearFactorGraph(nfg_); + } + + /// calculate gnc weights + Vector calculateWeights(const Values currentEstimate, const double mu){ + Vector weights = Vector::Ones(nfg_.size()); + return weights; + } }; -///* ************************************************************************* */ -//TEST(GncOptimizer, calculateWeights) { -//} -// -///* ************************************************************************* */ -//TEST(GncOptimizer, copyGraph) { -//} /* ************************************************************************* */ TEST(GncOptimizer, gncParamsConstructor) { @@ -106,7 +210,7 @@ TEST(GncOptimizer, gncParamsConstructor) { // and check params become different if we change lmParams lmParams.setVerbosity("DELTA"); - CHECK(!lmParams.equals(gncParams1.baseOptimizerParams)); + CHECK(! lmParams.equals(gncParams1.baseOptimizerParams)); // and same for GN GaussNewtonParams gnParams; @@ -116,9 +220,44 @@ TEST(GncOptimizer, gncParamsConstructor) { // check default constructor GncParams gncParams2b; CHECK(gnParams.equals(gncParams2b.baseOptimizerParams)); + + // change something at the gncParams level + GncParams gncParams2c(gncParams2b); + gncParams2c.setLossType(GncParams::RobustLossType::TLS); + CHECK(! gncParams2c.equals(gncParams2b.baseOptimizerParams)); } /* ************************************************************************* */ +TEST(GncOptimizer, gncConstructor) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + CHECK(gnc.getFactors().equals(fg)); + CHECK(gnc.getState().equals(initial)); + CHECK(gnc.getParams().equals(gncParams)); +} + +///* ************************************************************************* */ +//TEST(GncOptimizer, calculateWeights) { +//} +// +///* ************************************************************************* */ +//TEST(GncOptimizer, calculateWeights) { +//} +// +///* ************************************************************************* */ +//TEST(GncOptimizer, copyGraph) { +//} + +/* ************************************************************************* * TEST(GncOptimizer, makeGraph) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point @@ -134,7 +273,7 @@ TEST(GncOptimizer, makeGraph) { // NonlinearFactorGraph actual = gnc.makeGraph(initial); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST(GncOptimizer, optimize) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -144,10 +283,13 @@ TEST(GncOptimizer, optimize) { initial.insert(X(1), p0); LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); - auto gnc = GncOptimizer(fg, initial, gncParams); + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + gncParams.print(""); + Values actual = gnc.optimize(); - DOUBLES_EQUAL(0, fg.error(actual2), tol); + DOUBLES_EQUAL(0, fg.error(actual), tol); } /* ************************************************************************* */