From 7e29944f95c6afb4f9043b8c912add0f558b8852 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Wed, 25 Nov 2020 11:02:01 -0500 Subject: [PATCH 01/45] Initial design --- tests/testGncOptimizer.cpp | 115 +++++++++++++++++++++++++++++++++++++ 1 file changed, 115 insertions(+) create mode 100644 tests/testGncOptimizer.cpp diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp new file mode 100644 index 0000000000..d9ba209c5e --- /dev/null +++ b/tests/testGncOptimizer.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGncOptimizer.cpp + * @brief Unit tests for GncOptimizer class + * @author Jignnan Shi + * @author Luca Carlone + * @author Frank Dellaert + */ + +#include +#include +#include + +/* ************************************************************************* */ +template +class GncParams { + using BaseOptimizer = BaseOptimizerParameters::OptimizerType; + GncParams(const BaseOptimizerParameters& baseOptimizerParams) + : baseOptimizerParams(baseOptimizerParams) {} + + BaseOptimizerParameters baseOptimizerParams; + + /// any other specific GNC parameters: +}; + +/* ************************************************************************* */ +template +class GncOptimizer { + public: + // types etc + + private: + FG INITIAL GncParameters params_; + + public: + GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) { + // 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 makeGraph(const Values& currentSolution) const { + // calculate some weights + this->calculateWeights(); + // copy the graph with new weights + + } +}; + +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeights) { +} + +/* ************************************************************************* */ +TEST(GncOptimizer, copyGraph) { +} + +/* ************************************************************************* */ +TEST(GncOptimizer, makeGraph) { + // 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); + auto gnc = GncOptimizer(fg, initial, gncParams); + + NonlinearFactorGraph actual = gnc.makeGraph(initial); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, optimize) { + // 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); + auto gnc = GncOptimizer(fg, initial, gncParams); + Values actual = gnc.optimize(); + DOUBLES_EQUAL(0, fg.error(actual2), tol); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From b5d06b58786b066987d6e51c8aeffb3036a29c78 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Nov 2020 20:11:04 -0500 Subject: [PATCH 02/45] starting to create test and code for gncParams --- tests/testGncOptimizer.cpp | 107 ++++++++++++++++++++----------------- 1 file changed, 58 insertions(+), 49 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index d9ba209c5e..8788085052 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -12,7 +12,7 @@ /** * @file testGncOptimizer.cpp * @brief Unit tests for GncOptimizer class - * @author Jignnan Shi + * @author Jingnan Shi * @author Luca Carlone * @author Frank Dellaert */ @@ -21,12 +21,21 @@ #include #include +#include + +using namespace std; +using namespace gtsam; + +using symbol_shorthand::X; +using symbol_shorthand::L; + /* ************************************************************************* */ template class GncParams { - using BaseOptimizer = BaseOptimizerParameters::OptimizerType; - GncParams(const BaseOptimizerParameters& baseOptimizerParams) - : baseOptimizerParams(baseOptimizerParams) {} +public: + + // using BaseOptimizer = BaseOptimizerParameters::OptimizerType; + GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {} BaseOptimizerParameters baseOptimizerParams; @@ -34,64 +43,64 @@ class GncParams { }; /* ************************************************************************* */ -template -class GncOptimizer { - public: - // types etc - - private: - FG INITIAL GncParameters params_; - - public: - GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) { - // 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 makeGraph(const Values& currentSolution) const { - // calculate some weights - this->calculateWeights(); - // copy the graph with new weights - - } -}; - -/* ************************************************************************* */ -TEST(GncOptimizer, calculateWeights) { -} - -/* ************************************************************************* */ -TEST(GncOptimizer, copyGraph) { -} +//template +//class GncOptimizer { +// public: +// // types etc +// +// private: +// FG INITIAL GncParameters params_; +// +// public: +// GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) { +// // 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 makeGraph(const Values& currentSolution) const { +// // calculate some weights +// this->calculateWeights(); +// // copy the graph with new weights +// +// } +//}; + +///* ************************************************************************* */ +//TEST(GncOptimizer, calculateWeights) { +//} +// +///* ************************************************************************* */ +//TEST(GncOptimizer, copyGraph) { +//} /* ************************************************************************* */ TEST(GncOptimizer, makeGraph) { // has to have Gaussian noise models ! - auto fg = example::createReallyNonlinearFactorGraph(); + 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); + GncParams gncParams(lmParams); +// auto gnc = GncOptimizer(fg, initial, gncParams); - NonlinearFactorGraph actual = gnc.makeGraph(initial); +// NonlinearFactorGraph actual = gnc.makeGraph(initial); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST(GncOptimizer, optimize) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); From ff40590fc3dbf9963730fe4bdceda06d12914d71 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 12:59:27 -0500 Subject: [PATCH 03/45] added equals in NonlinearOptimizerParams --- gtsam/nonlinear/NonlinearOptimizerParams.h | 11 +++++++++++ tests/testNonlinearOptimizer.cpp | 13 +++++++++++++ 2 files changed, 24 insertions(+) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index a7bc10a1f4..218230421a 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -113,6 +113,17 @@ class GTSAM_EXPORT NonlinearOptimizerParams { virtual void print(const std::string& str = "") const; + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const { + return maxIterations == other.getMaxIterations() + && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol + && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol + && std::abs(errorTol - other.getErrorTol()) <= tol + && verbosityTranslator(verbosity) == other.getVerbosity(); + // && orderingType.equals(other.getOrderingType()_; + // && linearSolverType == other.getLinearSolverType(); + // TODO: check ordering, iterativeParams, and iterationsHook + } + inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 6415174d5b..295721cc42 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -48,6 +48,19 @@ const double tol = 1e-5; using symbol_shorthand::X; using symbol_shorthand::L; +/* ************************************************************************* */ +TEST( NonlinearOptimizer, paramsEquals ) +{ + // default constructors lead to two identical params + GaussNewtonParams gnParams1; + GaussNewtonParams gnParams2; + CHECK(gnParams1.equals(gnParams2)); + + // but the params become different if we change something in gnParams2 + gnParams2.setVerbosity("DELTA"); + CHECK(!gnParams1.equals(gnParams2)); +} + /* ************************************************************************* */ TEST( NonlinearOptimizer, iterateLM ) { From 90dd2c703548cb9c5ab6fcaa69bbde0423262941 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 13:05:54 -0500 Subject: [PATCH 04/45] params parsed correctly --- tests/testGncOptimizer.cpp | 90 ++++++++++++++++++++++++++------------ 1 file changed, 62 insertions(+), 28 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 8788085052..1d3057335c 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include @@ -37,44 +38,51 @@ class GncParams { // using BaseOptimizer = BaseOptimizerParameters::OptimizerType; GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {} + // default constructor + GncParams(): baseOptimizerParams() {} + BaseOptimizerParameters baseOptimizerParams; /// any other specific GNC parameters: }; /* ************************************************************************* */ -//template -//class GncOptimizer { -// public: -// // types etc -// -// private: -// FG INITIAL GncParameters params_; -// -// public: -// GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) { -// // Check that all noise models are Gaussian -// } -// +template +class GncOptimizer { +public: + // types etc + +private: + NonlinearFactorGraph nfg_; + Values state_; + GncParameters params_; + +public: + GncOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const GncParameters& params = GncParameters()) : + nfg_(graph), state_(initialValues), params_(params) { + // 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); +// for (i : {1, 2, 3}) { +// BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial); +// VALUES currentSolution = baseOptimizer.optimize(); +// if (converged) { +// return currentSolution; // } +// graph_i = this->makeGraph(currentSolution); // } +//} + +//NonlinearFactorGraph makeGraph(const Values& currentSolution) const { +// // calculate some weights +// this->calculateWeights(); +// // copy the graph with new weights // -// NonlinearFactorGraph makeGraph(const Values& currentSolution) const { -// // calculate some weights -// this->calculateWeights(); -// // copy the graph with new weights -// -// } -//}; +//} +}; ///* ************************************************************************* */ //TEST(GncOptimizer, calculateWeights) { @@ -84,6 +92,32 @@ class GncParams { //TEST(GncOptimizer, copyGraph) { //} +/* ************************************************************************* */ +TEST(GncOptimizer, gncParamsConstructor) { + + //check params are correctly parsed + LevenbergMarquardtParams lmParams; + GncParams gncParams1(lmParams); + CHECK(lmParams.equals(gncParams1.baseOptimizerParams)); + + // check also default constructor + GncParams gncParams1b; + CHECK(lmParams.equals(gncParams1b.baseOptimizerParams)); + + // and check params become different if we change lmParams + lmParams.setVerbosity("DELTA"); + CHECK(!lmParams.equals(gncParams1.baseOptimizerParams)); + + // and same for GN + GaussNewtonParams gnParams; + GncParams gncParams2(gnParams); + CHECK(gnParams.equals(gncParams2.baseOptimizerParams)); + + // check default constructor + GncParams gncParams2b; + CHECK(gnParams.equals(gncParams2b.baseOptimizerParams)); +} + /* ************************************************************************* */ TEST(GncOptimizer, makeGraph) { // has to have Gaussian noise models ! @@ -95,7 +129,7 @@ TEST(GncOptimizer, makeGraph) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); -// auto gnc = GncOptimizer(fg, initial, gncParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); // NonlinearFactorGraph actual = gnc.makeGraph(initial); } From f897fa81a948dcf13e27c002096a44dee40dab14 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 15:14:41 -0500 Subject: [PATCH 05/45] 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 1d3057335c..87ac268412 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 @@ class GncOptimizer { // 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_); } + + /// 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; + } -//NonlinearFactorGraph makeGraph(const Values& currentSolution) const { -// // calculate some weights -// this->calculateWeights(); -// // copy the graph with new weights -// -//} + /// 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); } /* ************************************************************************* */ From a33c50fcefb42dd85276624b83366d1aedfc0c46 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 15:46:12 -0500 Subject: [PATCH 06/45] now we have very cool tests! --- tests/smallExample.h | 47 ++++++++++++++++++++++++++++++++++++++ tests/testGncOptimizer.cpp | 37 +++++++++++++++++++++++++----- 2 files changed, 78 insertions(+), 6 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index 0c933d1060..271fb05812 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -363,6 +363,53 @@ inline NonlinearFactorGraph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } +/* ************************************************************************* */ +inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { + using symbol_shorthand::X; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(0.0, 0.0); + double sigma = 0.1; + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + // 3 noiseless inliers + fg->push_back(factor); + fg->push_back(factor); + fg->push_back(factor); + + // 1 outlier + Point2 z_out(1.0, 0.0); + boost::shared_ptr factor_out( + new smallOptimize::UnaryFactor(z_out, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor_out); + + return *fg; +} + +/* ************************************************************************* */ +inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { + using symbol_shorthand::X; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(0.0, 0.0); + double sigma = 0.1; + auto gmNoise = noiseModel::Robust::Create( + noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma)); + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, gmNoise, X(1))); + // 3 noiseless inliers + fg->push_back(factor); + fg->push_back(factor); + fg->push_back(factor); + + // 1 outlier + Point2 z_out(1.0, 0.0); + boost::shared_ptr factor_out( + new smallOptimize::UnaryFactor(z_out, gmNoise, X(1))); + fg->push_back(factor_out); + + return *fg; +} + + /* ************************************************************************* */ inline std::pair createNonlinearSmoother(int T) { using namespace impl; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 87ac268412..1951e51f11 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -253,9 +253,6 @@ TEST(GncOptimizer, gncConstructor) { //TEST(GncOptimizer, calculateWeights) { //} // -///* ************************************************************************* */ -//TEST(GncOptimizer, copyGraph) { -//} /* ************************************************************************* * TEST(GncOptimizer, makeGraph) { @@ -274,7 +271,7 @@ TEST(GncOptimizer, makeGraph) { } /* ************************************************************************* */ -TEST(GncOptimizer, optimize) { +TEST(GncOptimizer, optimizeSimple) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -286,12 +283,40 @@ TEST(GncOptimizer, optimize) { GncParams gncParams(lmParams); auto gnc = GncOptimizer>(fg, initial, gncParams); - gncParams.print(""); - Values actual = gnc.optimize(); DOUBLES_EQUAL(0, fg.error(actual), tol); } +/* ************************************************************************* */ +TEST(GncOptimizer, optimize) { + // has to have Gaussian noise models ! + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + + // try with nonrobust cost function and standard GN + GaussNewtonParams gnParams; + GaussNewtonOptimizer gn(fg, initial, gnParams); + Values gn_results = gn.optimize(); + // converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0) + CHECK(assert_equal(gn_results.at(X(1)), Point2(1.31812,0.0), 1e-3)); + + // try with robust loss function and standard GN + auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses + GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); + Values gn2_results = gn2.optimize(); + // converges to incorrect point, this time due to the nonconvexity of the loss + CHECK(assert_equal(gn2_results.at(X(1)), Point2(1.18712,0.0), 1e-3)); + + // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity + GncParams gncParams(gnParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(gnc_result.at(X(1)), Point2(0.0,0.0), 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 52225998fe64536ce42a514133cb92cc28b6a912 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 16:10:03 -0500 Subject: [PATCH 07/45] 2 tests to go --- tests/testGncOptimizer.cpp | 80 ++++++++++++++++++++++++++++++++++---- 1 file changed, 73 insertions(+), 7 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 1951e51f11..bfea5977ab 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -15,6 +15,9 @@ * @author Jingnan Shi * @author Luca Carlone * @author Frank Dellaert + * + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: + * From Non-Minimal Solvers to Global Outlier Rejection", RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) */ #include @@ -190,12 +193,23 @@ class GncOptimizer { /// calculate gnc weights Vector calculateWeights(const Values currentEstimate, const double mu){ - Vector weights = Vector::Ones(nfg_.size()); - return weights; + Vector weights = Vector::Zero(nfg_.size()); + switch(params_.lossType) { + case GncParameters::GM: + for (size_t k = 0; k < nfg_.size(); k++) { + if(nfg_[k]){ + double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + weights[k] = std::pow( ( mu*mu )/( u2_k + mu*mu ) , 2); + } + } + return weights; + default: + throw std::runtime_error( + "GncOptimizer::calculateWeights: called with unknown loss type."); + } } }; - /* ************************************************************************* */ TEST(GncOptimizer, gncParamsConstructor) { @@ -245,10 +259,62 @@ TEST(GncOptimizer, gncConstructor) { CHECK(gnc.getParams().equals(gncParams)); } -///* ************************************************************************* */ -//TEST(GncOptimizer, calculateWeights) { -//} -// +/* ************************************************************************* */ +TEST(GncOptimizer, initializeMu) { + // 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); + EXPECT_DOUBLES_EQUAL(gnc.initializeMu(), 2 * 198.999, 1e-3); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq (barcSq=1 in this example) +} + +/* ************************************************************************* */ +TEST(GncOptimizer, updateMu) { + // 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); + + double mu = 5.0; + EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol); + + // check it correctly saturates to 1 for GM + mu = 1.2; + EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), 1.0, tol); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkMuConvergence) { + // 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); + + double mu = 1.0; + CHECK(gnc.checkMuConvergence(mu)); +} + ///* ************************************************************************* */ //TEST(GncOptimizer, calculateWeights) { //} From 7c22c2c4027fe552831b295955011ddd9d9936e1 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 16:18:36 -0500 Subject: [PATCH 08/45] simplified small test to make it more understandable --- tests/smallExample.h | 17 +++++++++-------- tests/testGncOptimizer.cpp | 6 +++--- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index 271fb05812..70cda1eb0c 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -369,8 +369,9 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { boost::shared_ptr fg(new NonlinearFactorGraph); Point2 z(0.0, 0.0); double sigma = 0.1; - boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + + boost::shared_ptr> factor( + new PriorFactor(X(1), z, noiseModel::Isotropic::Sigma(2,sigma))); // 3 noiseless inliers fg->push_back(factor); fg->push_back(factor); @@ -378,8 +379,8 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { // 1 outlier Point2 z_out(1.0, 0.0); - boost::shared_ptr factor_out( - new smallOptimize::UnaryFactor(z_out, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + boost::shared_ptr> factor_out( + new PriorFactor(X(1), z_out, noiseModel::Isotropic::Sigma(2,sigma))); fg->push_back(factor_out); return *fg; @@ -393,8 +394,8 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { double sigma = 0.1; auto gmNoise = noiseModel::Robust::Create( noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma)); - boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, gmNoise, X(1))); + boost::shared_ptr> factor( + new PriorFactor(X(1), z, gmNoise)); // 3 noiseless inliers fg->push_back(factor); fg->push_back(factor); @@ -402,8 +403,8 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { // 1 outlier Point2 z_out(1.0, 0.0); - boost::shared_ptr factor_out( - new smallOptimize::UnaryFactor(z_out, gmNoise, X(1))); + boost::shared_ptr> factor_out( + new PriorFactor(X(1), z_out, gmNoise)); fg->push_back(factor_out); return *fg; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index bfea5977ab..d770f58a81 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -367,20 +367,20 @@ TEST(GncOptimizer, optimize) { GaussNewtonOptimizer gn(fg, initial, gnParams); Values gn_results = gn.optimize(); // converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0) - CHECK(assert_equal(gn_results.at(X(1)), Point2(1.31812,0.0), 1e-3)); + CHECK(assert_equal(Point2(0.25,0.0), gn_results.at(X(1)), 1e-3)); // try with robust loss function and standard GN auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); Values gn2_results = gn2.optimize(); // converges to incorrect point, this time due to the nonconvexity of the loss - CHECK(assert_equal(gn2_results.at(X(1)), Point2(1.18712,0.0), 1e-3)); + CHECK(assert_equal(Point2(0.999706,0.0), gn2_results.at(X(1)), 1e-3)); // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity GncParams gncParams(gnParams); auto gnc = GncOptimizer>(fg, initial, gncParams); Values gnc_result = gnc.optimize(); - CHECK(assert_equal(gnc_result.at(X(1)), Point2(0.0,0.0), 1e-3)); + CHECK(assert_equal(Point2(0.0,0.0), gnc_result.at(X(1)), 1e-3)); } /* ************************************************************************* */ From 0f07251cf515c8a06c56b3898933ff719f413fa6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 16:31:32 -0500 Subject: [PATCH 09/45] 1 test to go --- tests/testGncOptimizer.cpp | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index d770f58a81..a31f4b677f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -195,7 +195,7 @@ class GncOptimizer { Vector calculateWeights(const Values currentEstimate, const double mu){ Vector weights = Vector::Zero(nfg_.size()); switch(params_.lossType) { - case GncParameters::GM: + case GncParameters::GM: // use eq (12) in GNC paper for (size_t k = 0; k < nfg_.size(); k++) { if(nfg_[k]){ double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual @@ -315,10 +315,29 @@ TEST(GncOptimizer, checkMuConvergence) { CHECK(gnc.checkMuConvergence(mu)); } -///* ************************************************************************* */ -//TEST(GncOptimizer, calculateWeights) { -//} -// +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeights) { + // has to have Gaussian noise models ! + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(0, 0); + Values initial; + initial.insert(X(1), p0); + + // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) + Vector weights_expected = Vector::Zero(4); + weights_expected[0] = 1.0; // zero error + weights_expected[1] = 1.0; // zero error + weights_expected[2] = 1.0; // zero error + weights_expected[3] = std::pow(1.0 / (50.0 + 1.0),2); // outlier, error = 50 + + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + double mu = 1.0; + Vector weights_actual = gnc.calculateWeights(initial,mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); +} /* ************************************************************************* * TEST(GncOptimizer, makeGraph) { From e99188095fc017233dd0765beadd498c2e8fb1f5 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 17:14:34 -0500 Subject: [PATCH 10/45] stuck on conversion of noise model --- tests/testGncOptimizer.cpp | 68 +++++++++++++++++++++++++++++--------- 1 file changed, 53 insertions(+), 15 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a31f4b677f..a2dcafc817 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -113,7 +113,15 @@ class GncOptimizer { GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const GncParameters& params = GncParameters()) : nfg_(graph), state_(initialValues), params_(params) { - // TODO: Check that all noise models are Gaussian + + // make sure all noiseModels are Gaussian or convert to Gaussian + for (size_t i = 0; i < nfg_.size(); i++) { + if(nfg_[i]){ + // NonlinearFactor factor = nfg_[i]->clone(); + nfg_[i]-> + + } + } } NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); } @@ -199,7 +207,7 @@ class GncOptimizer { for (size_t k = 0; k < nfg_.size(); k++) { if(nfg_[k]){ double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - weights[k] = std::pow( ( mu*mu )/( u2_k + mu*mu ) , 2); + weights[k] = std::pow( ( mu*params_.barcSq )/( u2_k + mu*params_.barcSq ) , 2); } } return weights; @@ -259,6 +267,25 @@ TEST(GncOptimizer, gncConstructor) { CHECK(gnc.getParams().equals(gncParams)); } +/* ************************************************************************* */ +TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { + // simple graph with Gaussian noise model + auto fg = example::createReallyNonlinearFactorGraph(); + // same graph with robust noise model + auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg_robust, initial, gncParams); + + // make sure that when parsing the graph is transformed into one without robust loss + CHECK( fg.equals(gnc.getFactors()) ); +} + /* ************************************************************************* */ TEST(GncOptimizer, initializeMu) { // has to have Gaussian noise models ! @@ -337,22 +364,33 @@ TEST(GncOptimizer, calculateWeights) { double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial,mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); + + mu = 2.0; + double barcSq = 5.0; + weights_expected[3] = std::pow(mu*barcSq / (50.0 + mu*barcSq),2); // outlier, error = 50 + gncParams.setInlierThreshold(barcSq); + auto gnc2 = GncOptimizer>(fg, initial, gncParams); + weights_actual = gnc2.calculateWeights(initial,mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST(GncOptimizer, makeGraph) { - // 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); - -// NonlinearFactorGraph actual = gnc.makeGraph(initial); +// // simple graph with Gaussian noise model +// auto fg = example::createReallyNonlinearFactorGraph(); +// // same graph with robust noise model +// auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); +// +// Point2 p0(3, 3); +// Values initial; +// initial.insert(X(1), p0); +// +// LevenbergMarquardtParams lmParams; +// GncParams gncParams(lmParams); +// auto gnc = GncOptimizer>(fg_robust, initial, gncParams); +// +// // make sure that when parsing the graph is transformed into one without robust loss +// CHECK( fg.equals(gnc.getFactors()) ); } /* ************************************************************************* */ From 5db6894b66d387c051ba593c43c3b013482349d0 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 18:25:38 -0500 Subject: [PATCH 11/45] finally I have a way to properly change the noise model! --- gtsam/nonlinear/NonlinearFactor.cpp | 8 ++++++++ gtsam/nonlinear/NonlinearFactor.h | 6 ++++++ tests/smallExample.h | 15 +++++++++++++++ tests/testNonlinearFactor.cpp | 20 ++++++++++++++++++++ 4 files changed, 49 insertions(+) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 1cfcba274b..8b8d2da6c2 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -76,6 +76,14 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { && noiseModel_->equals(*e->noiseModel_, tol))); } +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel( + const SharedNoiseModel newNoise) const { + NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast(clone()); + new_factor->noiseModel_ = newNoise; + return new_factor; +} + /* ************************************************************************* */ static void check(const SharedNoiseModel& noiseModel, size_t m) { if (noiseModel && m != noiseModel->dim()) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 80fbfbb11c..00311fb87b 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -244,6 +244,12 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { */ boost::shared_ptr linearize(const Values& x) const override; + /** + * Creates a shared_ptr clone of the + * factor with a new noise model + */ + shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/tests/smallExample.h b/tests/smallExample.h index 70cda1eb0c..944899e701 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -342,10 +342,25 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { return (h(x) - z_); } + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } }; } +/* ************************************************************************* */ +inline NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma) { + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(1.0, 0.0); + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor); + return *fg; +} + /* ************************************************************************* */ inline boost::shared_ptr sharedReallyNonlinearFactorGraph() { using symbol_shorthand::X; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 662b071df8..84bba850b3 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -233,6 +233,26 @@ TEST( NonlinearFactor, linearize_constraint2 ) CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } +/* ************************************************************************* */ +TEST( NonlinearFactor, cloneWithNewNoiseModel ) +{ + // create original factor + double sigma1 = 0.1; + NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1); + + // create expected + double sigma2 = 10; + NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2); + + // create actual + NonlinearFactorGraph actual; + SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2); + actual.push_back( boost::dynamic_pointer_cast(nfg[0])->cloneWithNewNoiseModel(noise2) ); + + // check it's all good + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ class TestFactor4 : public NoiseModelFactor4 { public: From 7ce0641b4382c2d97cd8cd181a577bb5f3ad841d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 18:28:31 -0500 Subject: [PATCH 12/45] working on make graph --- tests/testGncOptimizer.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a2dcafc817..e80ad79743 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -112,14 +112,18 @@ class GncOptimizer { public: GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const GncParameters& params = GncParameters()) : - nfg_(graph), state_(initialValues), params_(params) { + state_(initialValues), params_(params) { // make sure all noiseModels are Gaussian or convert to Gaussian - for (size_t i = 0; i < nfg_.size(); i++) { - if(nfg_[i]){ - // NonlinearFactor factor = nfg_[i]->clone(); - nfg_[i]-> - + for (size_t i = 0; i < graph.size(); i++) { + if(graph[i]){ + auto &factor = boost::dynamic_pointer_cast(nfg_[i]); + auto &robust = boost::dynamic_pointer_cast(factor->noiseModel()); + if(robust){ // if the factor has a robust loss, we have to change it: + nfg_.push_back(factor->cloneWithNewNoiseModel(factor->noiseModel())); + }{ // else we directly push it back + nfg_.push_back(factor); + } } } } From 556fa83e9fbd866259b12b3a2a47def67ae12eb5 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 19:00:08 -0500 Subject: [PATCH 13/45] new constructor test which gets rid of robust loss now passes! --- tests/testGncOptimizer.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index e80ad79743..502eff520c 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -115,14 +115,17 @@ class GncOptimizer { state_(initialValues), params_(params) { // make sure all noiseModels are Gaussian or convert to Gaussian + nfg_.resize(graph.size()); for (size_t i = 0; i < graph.size(); i++) { if(graph[i]){ - auto &factor = boost::dynamic_pointer_cast(nfg_[i]); - auto &robust = boost::dynamic_pointer_cast(factor->noiseModel()); + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast(graph[i]); + noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast(factor->noiseModel()); if(robust){ // if the factor has a robust loss, we have to change it: - nfg_.push_back(factor->cloneWithNewNoiseModel(factor->noiseModel())); - }{ // else we directly push it back - nfg_.push_back(factor); + SharedNoiseModel gaussianNoise = robust->noise(); + NoiseModelFactor::shared_ptr gaussianFactor = factor->cloneWithNewNoiseModel(gaussianNoise); + nfg_[i] = gaussianFactor; + } else{ // else we directly push it back + nfg_[i] = factor; } } } @@ -274,7 +277,7 @@ TEST(GncOptimizer, gncConstructor) { /* ************************************************************************* */ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { // simple graph with Gaussian noise model - auto fg = example::createReallyNonlinearFactorGraph(); + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); // same graph with robust noise model auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); @@ -285,7 +288,9 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); auto gnc = GncOptimizer>(fg_robust, initial, gncParams); - +// fg.print("fg\n"); +// fg_robust.print("fg_robust\n"); +// gnc.getFactors().print("gnc\n"); // make sure that when parsing the graph is transformed into one without robust loss CHECK( fg.equals(gnc.getFactors()) ); } From 9e3263f2b1177d5770a6107ed31ca252bea1405c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 19:29:42 -0500 Subject: [PATCH 14/45] yay! only the final monster to go! --- tests/testGncOptimizer.cpp | 64 +++++++++++++++++++++++++++----------- 1 file changed, 45 insertions(+), 19 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 502eff520c..73584882fb 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -147,7 +147,7 @@ class GncOptimizer { weights = calculateWeights(result, mu); // variable/values update - NonlinearFactorGraph graph_iter = this->makeGraph(weights); + NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights); GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); Values result = baseOptimizer.optimize(); @@ -202,8 +202,25 @@ class GncOptimizer { } /// create a graph where each factor is weighted by the gnc weights - NonlinearFactorGraph makeGraph(const Vector& weights) const { - return NonlinearFactorGraph(nfg_); + NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const { + // make sure all noiseModels are Gaussian or convert to Gaussian + NonlinearFactorGraph newGraph; + newGraph.resize(nfg_.size()); + for (size_t i = 0; i < nfg_.size(); i++) { + if(nfg_[i]){ + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast(nfg_[i]); + noiseModel::Gaussian::shared_ptr noiseModel = boost::dynamic_pointer_cast(factor->noiseModel()); + if(noiseModel){ + Matrix newInfo = weights[i] * noiseModel->information(); + SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information(newInfo); + newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); + }else{ + throw std::runtime_error( + "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); + } + } + } + return newGraph; } /// calculate gnc weights @@ -384,22 +401,31 @@ TEST(GncOptimizer, calculateWeights) { } /* ************************************************************************* */ -TEST(GncOptimizer, makeGraph) { -// // simple graph with Gaussian noise model -// auto fg = example::createReallyNonlinearFactorGraph(); -// // same graph with robust noise model -// auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); -// -// Point2 p0(3, 3); -// Values initial; -// initial.insert(X(1), p0); -// -// LevenbergMarquardtParams lmParams; -// GncParams gncParams(lmParams); -// auto gnc = GncOptimizer>(fg_robust, initial, gncParams); -// -// // make sure that when parsing the graph is transformed into one without robust loss -// CHECK( fg.equals(gnc.getFactors()) ); +TEST(GncOptimizer, makeWeightedGraph) { + // create original factor + double sigma1 = 0.1; + NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1); + + // create expected + double sigma2 = 10; + NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2); + + // create weights + Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 + weights[0] = 1e-4; + + // create actual + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(nfg, initial, gncParams); + NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights); + + // check it's all good + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ From dab00907b9a6aac0802ed44ceb9eee2cd8be13da Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 22:07:16 -0500 Subject: [PATCH 15/45] added verbosity --- tests/testGncOptimizer.cpp | 49 ++++++++++++++++++++++++++++---------- 1 file changed, 36 insertions(+), 13 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 73584882fb..630b0c92b7 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -38,8 +38,12 @@ static double tol = 1e-7; template class GncParams { public: + /** Verbosity levels */ + enum VerbosityGNC { + SILENT = 0, SUMMARY, VALUES + }; - /** See NonlinearOptimizerParams::verbosity */ + /** Choice of robust loss function for GNC */ enum RobustLossType { GM /*Geman McClure*/, TLS /*Truncated least squares*/ }; @@ -51,7 +55,8 @@ class GncParams { 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 */ + muStep(1.4), /* multiplicative factor to reduce/increase the mu in gnc */ + verbosityGNC(SILENT){}/* verbosity level */ // default constructor GncParams(): baseOptimizerParams() {} @@ -62,16 +67,18 @@ class GncParams { size_t maxIterations; double barcSq; double muStep; + VerbosityGNC verbosityGNC; - void setLossType(RobustLossType type){ lossType = type; } - void setMaxIterations(size_t maxIter){ + void setLossType(const RobustLossType type){ lossType = type; } + void setMaxIterations(const 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; } + void setInlierThreshold(const double inth){ barcSq = inth; } + void setMuStep(const double step){ muStep = step; } + void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; } /// equals bool equals(const GncParams& other, double tol = 1e-9) const { @@ -79,7 +86,8 @@ class GncParams { && lossType == other.lossType && maxIterations == other.maxIterations && std::fabs(barcSq - other.barcSq) <= tol - && std::fabs(muStep - other.muStep) <= tol; + && std::fabs(muStep - other.muStep) <= tol + && verbosityGNC == other.verbosityGNC; } /// print function @@ -94,6 +102,7 @@ class GncParams { std::cout << "maxIterations: " << maxIterations << "\n"; std::cout << "barcSq: " << barcSq << "\n"; std::cout << "muStep: " << muStep << "\n"; + std::cout << "verbosityGNC: " << verbosityGNC << "\n"; baseOptimizerParams.print(str); } }; @@ -143,16 +152,31 @@ class GncOptimizer { Values result = baseOptimizer.optimize(); double mu = initializeMu(); for(size_t iter=0; iter < params_.maxIterations; iter++){ + + // display info + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES){ + result.print("result\n"); + std::cout << "mu: " << mu << std::endl; + std::cout << "weights: " << weights << std::endl; + } // weights update weights = calculateWeights(result, mu); // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights); GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); - Values result = baseOptimizer.optimize(); + result = baseOptimizer.optimize(); // stopping condition - if( checkMuConvergence(mu) ) { break; } + if( checkMuConvergence(mu) ) { + // 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; + } // otherwise update mu mu = updateMu(mu); @@ -160,7 +184,7 @@ class GncOptimizer { return result; } - /// initialize the gnc parameter mu such that loss is approximately convex + /// 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; @@ -305,9 +329,7 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); auto gnc = GncOptimizer>(fg_robust, initial, gncParams); -// fg.print("fg\n"); -// fg_robust.print("fg_robust\n"); -// gnc.getFactors().print("gnc\n"); + // make sure that when parsing the graph is transformed into one without robust loss CHECK( fg.equals(gnc.getFactors()) ); } @@ -470,6 +492,7 @@ TEST(GncOptimizer, optimize) { // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity GncParams gncParams(gnParams); + gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); 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)); From ef4774188136492116f067839e2d2122911d1b0d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 22:22:14 -0500 Subject: [PATCH 16/45] ladies and gents... GNC! --- tests/testGncOptimizer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 630b0c92b7..383544c6ef 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -48,7 +48,7 @@ class GncParams { GM /*Geman McClure*/, TLS /*Truncated least squares*/ }; - // using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; + using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams), @@ -165,7 +165,7 @@ class GncOptimizer { // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights); GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); - result = baseOptimizer.optimize(); + result = baseOptimizer_iter.optimize(); // stopping condition if( checkMuConvergence(mu) ) { @@ -492,7 +492,7 @@ TEST(GncOptimizer, optimize) { // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity GncParams gncParams(gnParams); - gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + // 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)); From c4644a0d61e76f1b895d8c3c8fe7a70be4e8d55d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 22:50:41 -0500 Subject: [PATCH 17/45] added functionality to fix weights --- tests/testGncOptimizer.cpp | 86 +++++++++++++++++++++++++++++--------- 1 file changed, 66 insertions(+), 20 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 383544c6ef..2e7692bec1 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -51,34 +51,34 @@ class GncParams { 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 */ - verbosityGNC(SILENT){}/* verbosity level */ + baseOptimizerParams(baseOptimizerParams) {} // default constructor GncParams(): baseOptimizerParams() {} BaseOptimizerParameters baseOptimizerParams; /// any other specific GNC parameters: - RobustLossType lossType; - size_t maxIterations; - double barcSq; - double muStep; - VerbosityGNC verbosityGNC; + RobustLossType lossType = GM; /* 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 */ + VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ + std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ void setLossType(const RobustLossType type){ lossType = type; } void setMaxIterations(const size_t maxIter){ std::cout - << "setMaxIterations: changing the max number of iterations might lead to less accurate solutions and is not recommended! " + << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " << std::endl; maxIterations = maxIter; } void setInlierThreshold(const double inth){ barcSq = inth; } void setMuStep(const double step){ muStep = step; } void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; } + void setKnownInliers(const std::vector knownIn) { + for(size_t i=0; i= GncParameters::VerbosityGNC::VALUES){ result.print("result\n"); std::cout << "mu: " << mu << std::endl; - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights_ << std::endl; } // weights update - weights = calculateWeights(result, mu); + weights_ = calculateWeights(result, mu); // variable/values update - NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights); + NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); result = baseOptimizer_iter.optimize(); @@ -173,7 +179,7 @@ class GncOptimizer { 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 << "final weights: " << weights_ << std::endl; } break; } @@ -249,10 +255,20 @@ class GncOptimizer { /// calculate gnc weights Vector calculateWeights(const Values currentEstimate, const double mu){ - Vector weights = Vector::Zero(nfg_.size()); + Vector weights = Vector::Ones(nfg_.size()); + + // do not update the weights that the user has decided are known inliers + std::vector allWeights; + for (size_t k = 0; k < nfg_.size(); k++) {allWeights.push_back(k);} + std::vector unknownWeights; + std::set_difference(allWeights.begin(), allWeights.end(), + params_.knownInliers.begin(), params_.knownInliers.end(), + std::inserter(unknownWeights, unknownWeights.begin())); + + // update weights of known inlier/outlier measurements switch(params_.lossType) { case GncParameters::GM: // use eq (12) in GNC paper - for (size_t k = 0; k < nfg_.size(); k++) { + for (size_t k : unknownWeights) { 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); @@ -498,6 +514,36 @@ TEST(GncOptimizer, optimize) { CHECK(assert_equal(Point2(0.0,0.0), gnc_result.at(X(1)), 1e-3)); } +/* ************************************************************************* */ +TEST(GncOptimizer, optimizeWithKnownInliers) { + // has to have Gaussian noise models ! + 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); + + // nonconvexity with known inliers + GncParams gncParams = GncParams(); + gncParams.setKnownInliers(knownInliers); + // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + 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); +} + /* ************************************************************************* */ int main() { TestResult tr; From 7699f04820cd94558a062666f7fdbb5b1502d3ba Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 22:54:51 -0500 Subject: [PATCH 18/45] correct formatting --- tests/testGncOptimizer.cpp | 283 +++++++++++++++++++++---------------- 1 file changed, 159 insertions(+), 124 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 2e7692bec1..65abc2042a 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -35,7 +35,7 @@ using symbol_shorthand::L; static double tol = 1e-7; /* ************************************************************************* */ -template +template class GncParams { public: /** Verbosity levels */ @@ -50,11 +50,14 @@ class GncParams { using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; - GncParams(const BaseOptimizerParameters& baseOptimizerParams): - baseOptimizerParams(baseOptimizerParams) {} + GncParams(const BaseOptimizerParameters& baseOptimizerParams) : + baseOptimizerParams(baseOptimizerParams) { + } // default constructor - GncParams(): baseOptimizerParams() {} + GncParams() : + baseOptimizerParams() { + } BaseOptimizerParameters baseOptimizerParams; /// any other specific GNC parameters: @@ -62,29 +65,36 @@ class GncParams { 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 */ - VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ + VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ - void setLossType(const RobustLossType type){ lossType = type; } - void setMaxIterations(const size_t maxIter){ + void setLossType(const RobustLossType type) { + lossType = type; + } + void setMaxIterations(const size_t maxIter) { std::cout - << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " - << std::endl; + << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " + << std::endl; maxIterations = maxIter; } - void setInlierThreshold(const double inth){ barcSq = inth; } - void setMuStep(const double step){ muStep = step; } - void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; } + void setInlierThreshold(const double inth) { + barcSq = inth; + } + void setMuStep(const double step) { + muStep = step; + } + void setVerbosityGNC(const VerbosityGNC verbosity) { + verbosityGNC = verbosity; + } void setKnownInliers(const std::vector knownIn) { - for(size_t i=0; i(graph[i]); - noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast(factor->noiseModel()); - if(robust){ // if the factor has a robust loss, we have to change it: + if (graph[i]) { + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + NoiseModelFactor>(graph[i]); + noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast< + noiseModel::Robust>(factor->noiseModel()); + if (robust) { // if the factor has a robust loss, we have to change it: SharedNoiseModel gaussianNoise = robust->noise(); - NoiseModelFactor::shared_ptr gaussianFactor = factor->cloneWithNewNoiseModel(gaussianNoise); + NoiseModelFactor::shared_ptr gaussianFactor = + factor->cloneWithNewNoiseModel(gaussianNoise); nfg_[i] = gaussianFactor; - } else{ // else we directly push it back + } else { // else we directly push it back nfg_[i] = factor; } } @@ -145,22 +159,30 @@ class GncOptimizer { } /// getter functions - NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); } - Values getState() const { return Values(state_); } - GncParameters getParams() const { return GncParameters(params_); } - Vector getWeights() const {return weights_;} + NonlinearFactorGraph getFactors() const { + return NonlinearFactorGraph(nfg_); + } + Values getState() const { + return Values(state_); + } + GncParameters getParams() const { + return GncParameters(params_); + } + Vector getWeights() const { + return weights_; + } /// implement GNC main loop, including graduating nonconvexity with mu Values optimize() { // start by assuming all measurements are inliers weights_ = Vector::Ones(nfg_.size()); - GaussNewtonOptimizer baseOptimizer(nfg_,state_); + GaussNewtonOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); - for(size_t iter=0; iter < params_.maxIterations; iter++){ + for (size_t iter = 0; iter < params_.maxIterations; iter++) { // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES){ + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { result.print("result\n"); std::cout << "mu: " << mu << std::endl; std::cout << "weights: " << weights_ << std::endl; @@ -174,9 +196,9 @@ class GncOptimizer { result = baseOptimizer_iter.optimize(); // stopping condition - if( checkMuConvergence(mu) ) { + if (checkMuConvergence(mu)) { // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY){ + 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; @@ -195,14 +217,14 @@ class GncOptimizer { // compute largest error across all factors double rmax_sq = 0.0; for (size_t i = 0; i < nfg_.size(); i++) { - if(nfg_[i]){ + if (nfg_[i]) { rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_)); } } // set initial mu - switch(params_.lossType) { + switch (params_.lossType) { case GncParameters::GM: - return 2*rmax_sq / params_.barcSq; // initial mu + return 2 * rmax_sq / params_.barcSq; // initial mu default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type."); @@ -211,9 +233,9 @@ class GncOptimizer { /// update the gnc parameter mu to gradually increase nonconvexity double updateMu(const double mu) const { - switch(params_.lossType) { + switch (params_.lossType) { case GncParameters::GM: - return std::max(1.0 , mu / params_.muStep); // reduce mu, but saturate at 1 + 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."); @@ -222,7 +244,7 @@ class GncOptimizer { /// 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) { + switch (params_.lossType) { case GncParameters::GM: return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function default: @@ -237,16 +259,20 @@ class GncOptimizer { NonlinearFactorGraph newGraph; newGraph.resize(nfg_.size()); for (size_t i = 0; i < nfg_.size(); i++) { - if(nfg_[i]){ - NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast(nfg_[i]); - noiseModel::Gaussian::shared_ptr noiseModel = boost::dynamic_pointer_cast(factor->noiseModel()); - if(noiseModel){ + if (nfg_[i]) { + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + NoiseModelFactor>(nfg_[i]); + noiseModel::Gaussian::shared_ptr noiseModel = + boost::dynamic_pointer_cast( + factor->noiseModel()); + if (noiseModel) { Matrix newInfo = weights[i] * noiseModel->information(); - SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information(newInfo); + SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information( + newInfo); newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); - }else{ + } else { throw std::runtime_error( - "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); + "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); } } } @@ -254,24 +280,27 @@ class GncOptimizer { } /// calculate gnc weights - Vector calculateWeights(const Values currentEstimate, const double mu){ + Vector calculateWeights(const Values currentEstimate, const double mu) { Vector weights = Vector::Ones(nfg_.size()); // do not update the weights that the user has decided are known inliers std::vector allWeights; - for (size_t k = 0; k < nfg_.size(); k++) {allWeights.push_back(k);} + for (size_t k = 0; k < nfg_.size(); k++) { + allWeights.push_back(k); + } std::vector unknownWeights; std::set_difference(allWeights.begin(), allWeights.end(), params_.knownInliers.begin(), params_.knownInliers.end(), std::inserter(unknownWeights, unknownWeights.begin())); // update weights of known inlier/outlier measurements - switch(params_.lossType) { + switch (params_.lossType) { case GncParameters::GM: // use eq (12) in GNC paper for (size_t k : unknownWeights) { - if(nfg_[k]){ + 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); + weights[k] = std::pow( + (mu * params_.barcSq) / (u2_k + mu * params_.barcSq), 2); } } return weights; @@ -284,7 +313,6 @@ class GncOptimizer { /* ************************************************************************* */ TEST(GncOptimizer, gncParamsConstructor) { - //check params are correctly parsed LevenbergMarquardtParams lmParams; GncParams gncParams1(lmParams); @@ -296,7 +324,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; @@ -310,7 +338,7 @@ TEST(GncOptimizer, gncParamsConstructor) { // change something at the gncParams level GncParams gncParams2c(gncParams2b); gncParams2c.setLossType(GncParams::RobustLossType::TLS); - CHECK(! gncParams2c.equals(gncParams2b.baseOptimizerParams)); + CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); } /* ************************************************************************* */ @@ -324,7 +352,8 @@ TEST(GncOptimizer, gncConstructor) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(fg, initial, gncParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); CHECK(gnc.getFactors().equals(fg)); CHECK(gnc.getState().equals(initial)); @@ -333,7 +362,6 @@ TEST(GncOptimizer, gncConstructor) { /* ************************************************************************* */ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { - // simple graph with Gaussian noise model auto fg = example::sharedNonRobustFactorGraphWithOutliers(); // same graph with robust noise model auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); @@ -344,15 +372,15 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(fg_robust, initial, gncParams); + auto gnc = GncOptimizer>(fg_robust, + initial, gncParams); // make sure that when parsing the graph is transformed into one without robust loss - CHECK( fg.equals(gnc.getFactors()) ); + CHECK(fg.equals(gnc.getFactors())); } /* ************************************************************************* */ TEST(GncOptimizer, initializeMu) { - // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); Point2 p0(3, 3); @@ -361,8 +389,10 @@ TEST(GncOptimizer, initializeMu) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - gncParams.setLossType(GncParams::RobustLossType::GM); - auto gnc = GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); EXPECT_DOUBLES_EQUAL(gnc.initializeMu(), 2 * 198.999, 1e-3); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq (barcSq=1 in this example) } @@ -377,8 +407,10 @@ TEST(GncOptimizer, updateMu) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - gncParams.setLossType(GncParams::RobustLossType::GM); - auto gnc = GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); double mu = 5.0; EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol); @@ -399,8 +431,10 @@ TEST(GncOptimizer, checkMuConvergence) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - gncParams.setLossType(GncParams::RobustLossType::GM); - auto gnc = GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); double mu = 1.0; CHECK(gnc.checkMuConvergence(mu)); @@ -408,67 +442,69 @@ TEST(GncOptimizer, checkMuConvergence) { /* ************************************************************************* */ TEST(GncOptimizer, calculateWeights) { - // has to have Gaussian noise models ! - auto fg = example::sharedNonRobustFactorGraphWithOutliers(); - - Point2 p0(0, 0); - Values initial; - initial.insert(X(1), p0); - - // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) - Vector weights_expected = Vector::Zero(4); - weights_expected[0] = 1.0; // zero error - weights_expected[1] = 1.0; // zero error - weights_expected[2] = 1.0; // zero error - weights_expected[3] = std::pow(1.0 / (50.0 + 1.0),2); // outlier, error = 50 - - GaussNewtonParams gnParams; - GncParams gncParams(gnParams); - auto gnc = GncOptimizer>(fg, initial, gncParams); - double mu = 1.0; - Vector weights_actual = gnc.calculateWeights(initial,mu); - CHECK(assert_equal(weights_expected, weights_actual, tol)); - - mu = 2.0; - double barcSq = 5.0; - weights_expected[3] = std::pow(mu*barcSq / (50.0 + mu*barcSq),2); // outlier, error = 50 - gncParams.setInlierThreshold(barcSq); - auto gnc2 = GncOptimizer>(fg, initial, gncParams); - weights_actual = gnc2.calculateWeights(initial,mu); - CHECK(assert_equal(weights_expected, weights_actual, tol)); + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(0, 0); + Values initial; + initial.insert(X(1), p0); + + // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) + Vector weights_expected = Vector::Zero(4); + weights_expected[0] = 1.0; // zero error + weights_expected[1] = 1.0; // zero error + weights_expected[2] = 1.0; // zero error + weights_expected[3] = std::pow(1.0 / (50.0 + 1.0), 2); // outlier, error = 50 + + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + double mu = 1.0; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); + + mu = 2.0; + double barcSq = 5.0; + weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 + gncParams.setInlierThreshold(barcSq); + auto gnc2 = GncOptimizer>(fg, initial, + gncParams); + weights_actual = gnc2.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); } /* ************************************************************************* */ TEST(GncOptimizer, makeWeightedGraph) { // create original factor - double sigma1 = 0.1; - NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1); + double sigma1 = 0.1; + NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma( + sigma1); - // create expected - double sigma2 = 10; - NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2); + // create expected + double sigma2 = 10; + NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma( + sigma2); - // create weights - Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 - weights[0] = 1e-4; + // create weights + Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 + weights[0] = 1e-4; - // create actual - Point2 p0(3, 3); - Values initial; - initial.insert(X(1), p0); + // create actual + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(nfg, initial, gncParams); - NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights); + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(nfg, initial, + gncParams); + NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights); - // check it's all good - CHECK(assert_equal(expected, actual)); + // check it's all good + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(GncOptimizer, optimizeSimple) { - // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); Point2 p0(3, 3); @@ -477,7 +513,8 @@ TEST(GncOptimizer, optimizeSimple) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(fg, initial, gncParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); Values actual = gnc.optimize(); DOUBLES_EQUAL(0, fg.error(actual), tol); @@ -485,7 +522,6 @@ TEST(GncOptimizer, optimizeSimple) { /* ************************************************************************* */ TEST(GncOptimizer, optimize) { - // has to have Gaussian noise models ! auto fg = example::sharedNonRobustFactorGraphWithOutliers(); Point2 p0(1, 0); @@ -497,26 +533,25 @@ TEST(GncOptimizer, optimize) { GaussNewtonOptimizer gn(fg, initial, gnParams); Values gn_results = gn.optimize(); // converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0) - CHECK(assert_equal(Point2(0.25,0.0), gn_results.at(X(1)), 1e-3)); + CHECK(assert_equal(Point2(0.25, 0.0), gn_results.at(X(1)), 1e-3)); // try with robust loss function and standard GN auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); Values gn2_results = gn2.optimize(); // converges to incorrect point, this time due to the nonconvexity of the loss - CHECK(assert_equal(Point2(0.999706,0.0), gn2_results.at(X(1)), 1e-3)); + CHECK(assert_equal(Point2(0.999706, 0.0), gn2_results.at(X(1)), 1e-3)); // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity GncParams gncParams(gnParams); // 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(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); } /* ************************************************************************* */ TEST(GncOptimizer, optimizeWithKnownInliers) { - // has to have Gaussian noise models ! auto fg = example::sharedNonRobustFactorGraphWithOutliers(); Point2 p0(1, 0); @@ -535,7 +570,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { 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(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); // check weights were actually fixed: Vector finalWeights = gnc.getWeights(); From 786d4bbf9a6609b536493a6e4fe1c64a3f1fccda Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 23:12:26 -0500 Subject: [PATCH 19/45] done - PGO works like a charm! --- tests/testGncOptimizer.cpp | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 65abc2042a..8415ec3cce 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -20,6 +20,8 @@ * From Non-Minimal Solvers to Global Outlier Rejection", RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) */ +#include + #include #include #include @@ -579,6 +581,42 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { DOUBLES_EQUAL(1.0, finalWeights[2], tol); } +/* ************************************************************************* */ +TEST(GncOptimizer, optimizeSmallPoseGraph) { + /// load small pose graph + const string filename = findExampleDataFile("w100.graph"); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = load2D(filename); + // Add a Gaussian prior on first poses + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); + graph -> addPrior(0, priorMean, priorNoise); + + /// get expected values by optimizing outlier-free graph + Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + + // add a few outliers + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01)); + graph->push_back( BetweenFactor(90 , 50 , Pose2(), betweenNoise) ); // some arbitrary and incorrect between factor + + /// get expected values by optimizing outlier-free graph + Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + // as expected, the following test fails due to the presence of an outlier! + // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); + + // GNC + // Note: in difficult instances, we set the odometry measurements to be inliers, + // but this problem is simple enought to succeed even without that assumption + // std::vector knownInliers; + GncParams gncParams = GncParams(); + auto gnc = GncOptimizer>(*graph, *initial, gncParams); + Values actual = gnc.optimize(); + + // compare + CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! +} + /* ************************************************************************* */ int main() { TestResult tr; From fcf2d316848cb54b50513af27f7a1f00501b119b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 5 Dec 2020 13:47:40 -0500 Subject: [PATCH 20/45] moved class to .h --- gtsam/nonlinear/GncOptimizer.h | 312 +++++++++++++++++++++++++++++++++ tests/testGncOptimizer.cpp | 293 +------------------------------ 2 files changed, 320 insertions(+), 285 deletions(-) create mode 100644 gtsam/nonlinear/GncOptimizer.h diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h new file mode 100644 index 0000000000..6601494e87 --- /dev/null +++ b/gtsam/nonlinear/GncOptimizer.h @@ -0,0 +1,312 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGncOptimizer.cpp + * @brief Unit tests for GncOptimizer class + * @author Jingnan Shi + * @author Luca Carlone + * @author Frank Dellaert + * + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: + * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * + * See also: + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", + * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +class GncParams { +public: + /** Verbosity levels */ + enum VerbosityGNC { + SILENT = 0, SUMMARY, VALUES + }; + + /** Choice of robust loss function for GNC */ + enum RobustLossType { + GM /*Geman McClure*/, TLS /*Truncated least squares*/ + }; + + using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; + + GncParams(const BaseOptimizerParameters& baseOptimizerParams) : + baseOptimizerParams(baseOptimizerParams) { + } + + // default constructor + GncParams() : + baseOptimizerParams() { + } + + BaseOptimizerParameters baseOptimizerParams; + /// any other specific GNC parameters: + RobustLossType lossType = GM; /* 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 */ + VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ + std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ + + void setLossType(const RobustLossType type) { + lossType = type; + } + void setMaxIterations(const size_t maxIter) { + std::cout + << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " + << std::endl; + maxIterations = maxIter; + } + void setInlierThreshold(const double inth) { + barcSq = inth; + } + void setMuStep(const double step) { + muStep = step; + } + void setVerbosityGNC(const VerbosityGNC verbosity) { + verbosityGNC = verbosity; + } + void setKnownInliers(const std::vector knownIn) { + for (size_t i = 0; i < knownIn.size(); i++) + knownInliers.push_back(knownIn[i]); + } + + /// 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 + && verbosityGNC == other.verbosityGNC + && knownInliers == other.knownInliers; + } + + /// 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"; + std::cout << "verbosityGNC: " << verbosityGNC << "\n"; + for (size_t i = 0; i < knownInliers.size(); i++) + std::cout << "knownInliers: " << knownInliers[i] << "\n"; + baseOptimizerParams.print(str); + } +}; + +/* ************************************************************************* */ +template +class GncOptimizer { +public: + // types etc + +private: + NonlinearFactorGraph nfg_; + Values state_; + GncParameters params_; + Vector weights_; // this could be a local variable in optimize, but it is useful to make it accessible from outside + +public: + GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const GncParameters& params = GncParameters()) : + state_(initialValues), params_(params) { + + // make sure all noiseModels are Gaussian or convert to Gaussian + nfg_.resize(graph.size()); + for (size_t i = 0; i < graph.size(); i++) { + if (graph[i]) { + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + NoiseModelFactor>(graph[i]); + noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast< + noiseModel::Robust>(factor->noiseModel()); + if (robust) { // if the factor has a robust loss, we have to change it: + SharedNoiseModel gaussianNoise = robust->noise(); + NoiseModelFactor::shared_ptr gaussianFactor = + factor->cloneWithNewNoiseModel(gaussianNoise); + nfg_[i] = gaussianFactor; + } else { // else we directly push it back + nfg_[i] = factor; + } + } + } + } + + /// getter functions + NonlinearFactorGraph getFactors() const { + return NonlinearFactorGraph(nfg_); + } + Values getState() const { + return Values(state_); + } + GncParameters getParams() const { + return GncParameters(params_); + } + Vector getWeights() const { + return weights_; + } + + /// implement GNC main loop, including graduating nonconvexity with mu + Values optimize() { + // start by assuming all measurements are inliers + 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++) { + + // display info + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + result.print("result\n"); + std::cout << "mu: " << mu << std::endl; + std::cout << "weights: " << weights_ << std::endl; + } + // weights update + weights_ = calculateWeights(result, mu); + + // variable/values update + NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); + GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); + result = baseOptimizer_iter.optimize(); + + // stopping condition + if (checkMuConvergence(mu)) { + // 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; + } + + // otherwise update mu + mu = updateMu(mu); + } + return result; + } + + /// 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_)); + } + } + // 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 makeWeightedGraph(const Vector& weights) const { + // make sure all noiseModels are Gaussian or convert to Gaussian + NonlinearFactorGraph newGraph; + newGraph.resize(nfg_.size()); + for (size_t i = 0; i < nfg_.size(); i++) { + if (nfg_[i]) { + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + NoiseModelFactor>(nfg_[i]); + noiseModel::Gaussian::shared_ptr noiseModel = + boost::dynamic_pointer_cast( + factor->noiseModel()); + if (noiseModel) { + Matrix newInfo = weights[i] * noiseModel->information(); + SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information( + newInfo); + newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); + } else { + throw std::runtime_error( + "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); + } + } + } + return newGraph; + } + + /// calculate gnc weights + Vector calculateWeights(const Values currentEstimate, const double mu) { + Vector weights = Vector::Ones(nfg_.size()); + + // do not update the weights that the user has decided are known inliers + std::vector allWeights; + for (size_t k = 0; k < nfg_.size(); k++) { + allWeights.push_back(k); + } + std::vector unknownWeights; + std::set_difference(allWeights.begin(), allWeights.end(), + params_.knownInliers.begin(), params_.knownInliers.end(), + std::inserter(unknownWeights, unknownWeights.begin())); + + // update weights of known inlier/outlier measurements + switch (params_.lossType) { + case GncParameters::GM: // use eq (12) in GNC paper + for (size_t k : unknownWeights) { + 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); + } + } + return weights; + default: + throw std::runtime_error( + "GncOptimizer::calculateWeights: called with unknown loss type."); + } + } +}; + +} diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 8415ec3cce..5006aa941b 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -17,16 +17,16 @@ * @author Frank Dellaert * * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: - * From Non-Minimal Solvers to Global Outlier Rejection", RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * + * See also: + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", + * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. */ #include - -#include -#include -#include +#include #include - #include using namespace std; @@ -36,283 +36,6 @@ using symbol_shorthand::X; using symbol_shorthand::L; static double tol = 1e-7; -/* ************************************************************************* */ -template -class GncParams { -public: - /** Verbosity levels */ - enum VerbosityGNC { - SILENT = 0, SUMMARY, VALUES - }; - - /** Choice of robust loss function for GNC */ - enum RobustLossType { - GM /*Geman McClure*/, TLS /*Truncated least squares*/ - }; - - using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; - - GncParams(const BaseOptimizerParameters& baseOptimizerParams) : - baseOptimizerParams(baseOptimizerParams) { - } - - // default constructor - GncParams() : - baseOptimizerParams() { - } - - BaseOptimizerParameters baseOptimizerParams; - /// any other specific GNC parameters: - RobustLossType lossType = GM; /* 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 */ - VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ - std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ - - void setLossType(const RobustLossType type) { - lossType = type; - } - void setMaxIterations(const size_t maxIter) { - std::cout - << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " - << std::endl; - maxIterations = maxIter; - } - void setInlierThreshold(const double inth) { - barcSq = inth; - } - void setMuStep(const double step) { - muStep = step; - } - void setVerbosityGNC(const VerbosityGNC verbosity) { - verbosityGNC = verbosity; - } - void setKnownInliers(const std::vector knownIn) { - for (size_t i = 0; i < knownIn.size(); i++) - knownInliers.push_back(knownIn[i]); - } - - /// 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 - && verbosityGNC == other.verbosityGNC - && knownInliers == other.knownInliers; - } - - /// 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"; - std::cout << "verbosityGNC: " << verbosityGNC << "\n"; - for (size_t i = 0; i < knownInliers.size(); i++) - std::cout << "knownInliers: " << knownInliers[i] << "\n"; - baseOptimizerParams.print(str); - } -}; - -/* ************************************************************************* */ -template -class GncOptimizer { -public: - // types etc - -private: - NonlinearFactorGraph nfg_; - Values state_; - GncParameters params_; - Vector weights_; // this could be a local variable in optimize, but it is useful to make it accessible from outside - -public: - GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const GncParameters& params = GncParameters()) : - state_(initialValues), params_(params) { - - // make sure all noiseModels are Gaussian or convert to Gaussian - nfg_.resize(graph.size()); - for (size_t i = 0; i < graph.size(); i++) { - if (graph[i]) { - NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< - NoiseModelFactor>(graph[i]); - noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast< - noiseModel::Robust>(factor->noiseModel()); - if (robust) { // if the factor has a robust loss, we have to change it: - SharedNoiseModel gaussianNoise = robust->noise(); - NoiseModelFactor::shared_ptr gaussianFactor = - factor->cloneWithNewNoiseModel(gaussianNoise); - nfg_[i] = gaussianFactor; - } else { // else we directly push it back - nfg_[i] = factor; - } - } - } - } - - /// getter functions - NonlinearFactorGraph getFactors() const { - return NonlinearFactorGraph(nfg_); - } - Values getState() const { - return Values(state_); - } - GncParameters getParams() const { - return GncParameters(params_); - } - Vector getWeights() const { - return weights_; - } - - /// implement GNC main loop, including graduating nonconvexity with mu - Values optimize() { - // start by assuming all measurements are inliers - 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++) { - - // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { - result.print("result\n"); - std::cout << "mu: " << mu << std::endl; - std::cout << "weights: " << weights_ << std::endl; - } - // weights update - weights_ = calculateWeights(result, mu); - - // variable/values update - NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); - GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); - result = baseOptimizer_iter.optimize(); - - // stopping condition - if (checkMuConvergence(mu)) { - // 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; - } - - // otherwise update mu - mu = updateMu(mu); - } - return result; - } - - /// 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_)); - } - } - // 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 makeWeightedGraph(const Vector& weights) const { - // make sure all noiseModels are Gaussian or convert to Gaussian - NonlinearFactorGraph newGraph; - newGraph.resize(nfg_.size()); - for (size_t i = 0; i < nfg_.size(); i++) { - if (nfg_[i]) { - NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< - NoiseModelFactor>(nfg_[i]); - noiseModel::Gaussian::shared_ptr noiseModel = - boost::dynamic_pointer_cast( - factor->noiseModel()); - if (noiseModel) { - Matrix newInfo = weights[i] * noiseModel->information(); - SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information( - newInfo); - newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); - } else { - throw std::runtime_error( - "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); - } - } - } - return newGraph; - } - - /// calculate gnc weights - Vector calculateWeights(const Values currentEstimate, const double mu) { - Vector weights = Vector::Ones(nfg_.size()); - - // do not update the weights that the user has decided are known inliers - std::vector allWeights; - for (size_t k = 0; k < nfg_.size(); k++) { - allWeights.push_back(k); - } - std::vector unknownWeights; - std::set_difference(allWeights.begin(), allWeights.end(), - params_.knownInliers.begin(), params_.knownInliers.end(), - std::inserter(unknownWeights, unknownWeights.begin())); - - // update weights of known inlier/outlier measurements - switch (params_.lossType) { - case GncParameters::GM: // use eq (12) in GNC paper - for (size_t k : unknownWeights) { - 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); - } - } - return weights; - default: - throw std::runtime_error( - "GncOptimizer::calculateWeights: called with unknown loss type."); - } - } -}; - /* ************************************************************************* */ TEST(GncOptimizer, gncParamsConstructor) { //check params are correctly parsed @@ -566,7 +289,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { knownInliers.push_back(2); // nonconvexity with known inliers - GncParams gncParams = GncParams(); + GncParams gncParams; gncParams.setKnownInliers(knownInliers); // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -609,7 +332,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // Note: in difficult instances, we set the odometry measurements to be inliers, // but this problem is simple enought to succeed even without that assumption // std::vector knownInliers; - GncParams gncParams = GncParams(); + GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); Values actual = gnc.optimize(); From db1a3668483e7a94804f9e2d05e254d081cf8ccf Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 5 Dec 2020 14:06:32 -0500 Subject: [PATCH 21/45] added comments --- gtsam/nonlinear/GncOptimizer.h | 31 +++++++++++++++++++++++-------- 1 file changed, 23 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 6601494e87..7c41b24755 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -48,16 +48,18 @@ class GncParams { using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; + /// Constructor GncParams(const BaseOptimizerParameters& baseOptimizerParams) : baseOptimizerParams(baseOptimizerParams) { } - // default constructor + /// Default constructor GncParams() : baseOptimizerParams() { } - BaseOptimizerParameters baseOptimizerParams; + /// 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*/ size_t maxIterations = 100; /* maximum number of iterations*/ @@ -66,29 +68,41 @@ class GncParams { VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ + /// Set the robust loss function to be used in GNC (chosen among the ones in RobustLossType) void setLossType(const RobustLossType type) { lossType = type; } + /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended) void setMaxIterations(const size_t maxIter) { std::cout << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " << std::endl; 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. + * */ void setInlierThreshold(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; } + /// Set the verbosity level void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; } + /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector + * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, + * and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. + * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and + * only apply GNC to prune outliers from the loop closures + * */ void setKnownInliers(const std::vector knownIn) { for (size_t i = 0; i < knownIn.size(); i++) knownInliers.push_back(knownIn[i]); } - /// equals bool equals(const GncParams& other, double tol = 1e-9) const { return baseOptimizerParams.equals(other.baseOptimizerParams) @@ -98,7 +112,6 @@ class GncParams { && verbosityGNC == other.verbosityGNC && knownInliers == other.knownInliers; } - /// print function void print(const std::string& str) const { std::cout << str << "\n"; @@ -132,6 +145,7 @@ class GncOptimizer { Vector weights_; // this could be a local variable in optimize, but it is useful to make it accessible from outside public: + /// Constructor GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const GncParameters& params = GncParameters()) : state_(initialValues), params_(params) { @@ -156,21 +170,23 @@ class GncOptimizer { } } - /// getter functions + /// Access a copy of the internal factor graph NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); } + /// Access a copy of the internal values Values getState() const { return Values(state_); } + /// Access a copy of the parameters GncParameters getParams() const { return GncParameters(params_); } + /// Access a copy of the GNC weights Vector getWeights() const { return weights_; } - - /// implement GNC main loop, including graduating nonconvexity with mu + /// Compute optimal solution using graduated non-convexity Values optimize() { // start by assuming all measurements are inliers weights_ = Vector::Ones(nfg_.size()); @@ -203,7 +219,6 @@ class GncOptimizer { } break; } - // otherwise update mu mu = updateMu(mu); } From af069ab4b28bc31fe8e0c06e060c5d687bc954ce Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Sun, 6 Dec 2020 13:50:44 -0500 Subject: [PATCH 22/45] fix comment --- gtsam/nonlinear/GncOptimizer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 7c41b24755..d540dffbb6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testGncOptimizer.cpp - * @brief Unit tests for GncOptimizer class + * @file GncOptimizer.cpp + * @brief The GncOptimizer class * @author Jingnan Shi * @author Luca Carlone * @author Frank Dellaert From 47775a7a4f7134c382e7b47c0d340e9c65a4fc7e Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 00:53:21 -0500 Subject: [PATCH 23/45] TLS wip --- gtsam/nonlinear/GncOptimizer.h | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d540dffbb6..be7d046c66 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -119,6 +119,9 @@ class GncParams { case GM: std::cout << "lossType: Geman McClure" << "\n"; break; + case TLS: + std::cout << "lossType: Truncated Least-squares" << "\n"; + break; default: throw std::runtime_error("GncParams::print: unknown loss type."); } @@ -193,6 +196,18 @@ class GncOptimizer { GaussNewtonOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); + + // handle the degenerate case for TLS cost that corresponds to small + // maximum residual error at initialization + if (mu <= 0 && params_.lossType == GncParameters::TLS) { + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { + std::cout << "GNC Optimizer stopped because maximum residual at " + "initialization is small." << std::endl; + result.print("result\n"); + } + return result; + } + for (size_t iter = 0; iter < params_.maxIterations; iter++) { // display info @@ -238,6 +253,11 @@ class GncOptimizer { switch (params_.lossType) { case GncParameters::GM: return 2 * rmax_sq / params_.barcSq; // initial mu + case GncParameters::TLS: + // initialize mu to the value specified in Remark 5 in GNC paper + // degenerate case: residual is close to zero so mu approximately equals + // to -1 + return params_.barcSq / (2 * rmax_sq - params_.barcSq); default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type."); @@ -249,6 +269,9 @@ class GncOptimizer { switch (params_.lossType) { case GncParameters::GM: return std::max(1.0, mu / params_.muStep); // reduce mu, but saturate at 1 + case GncParameters::TLS: + // increases mu at each iteration + return mu * params_.muStep; default: throw std::runtime_error( "GncOptimizer::updateMu: called with unknown loss type."); @@ -260,6 +283,7 @@ class GncOptimizer { switch (params_.lossType) { case GncParameters::GM: return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + // TODO: Add TLS default: throw std::runtime_error( "GncOptimizer::checkMuConvergence: called with unknown loss type."); @@ -317,6 +341,7 @@ class GncOptimizer { } } return weights; + // TODO: Add TLS default: throw std::runtime_error( "GncOptimizer::calculateWeights: called with unknown loss type."); From 9903fb91d0ceaa65885a2c42282e0563bb104d63 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 13:24:49 -0500 Subject: [PATCH 24/45] tls done except unit tests --- gtsam/nonlinear/GncOptimizer.h | 31 +++++++++++++++++++++++++++---- tests/testGncOptimizer.cpp | 4 +++- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index be7d046c66..b6e6933ec7 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -65,6 +65,7 @@ class GncParams { 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 relativeMuTol = 1e-5; ///< The maximum relative mu decrease to stop iterating VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ @@ -89,6 +90,10 @@ class GncParams { void setMuStep(const double step) { muStep = step; } + /// Set the maximum relative difference in mu values to stop iterating + void setRelativeMuTol(double value) { + relativeMuTol = value; + } /// Set the verbosity level void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; @@ -196,6 +201,7 @@ class GncOptimizer { GaussNewtonOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); + double mu_prev = mu; // handle the degenerate case for TLS cost that corresponds to small // maximum residual error at initialization @@ -225,7 +231,7 @@ class GncOptimizer { result = baseOptimizer_iter.optimize(); // stopping condition - if (checkMuConvergence(mu)) { + if (checkMuConvergence(mu, mu_prev)) { // display info if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; @@ -235,6 +241,7 @@ class GncOptimizer { break; } // otherwise update mu + mu_prev = mu; mu = updateMu(mu); } return result; @@ -279,11 +286,12 @@ class GncOptimizer { } /// 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 checkMuConvergence(const double mu, const double mu_prev) const { switch (params_.lossType) { case GncParameters::GM: return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function - // TODO: Add TLS + case GncParameters::TLS: + return std::fabs(mu - mu_prev) < params_.relativeMuTol; default: throw std::runtime_error( "GncOptimizer::checkMuConvergence: called with unknown loss type."); @@ -341,7 +349,22 @@ class GncOptimizer { } } return weights; - // TODO: Add TLS + case GncParameters::TLS: // use eq (14) in GNC paper + double upperbound = (mu + 1) / mu * params_.barcSq; + double lowerbound = mu / (mu +1 ) * params_.barcSq; + for (size_t k : unknownWeights) { + if (nfg_[k]) { + double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + if (u2_k >= upperbound ) { + weights[k] = 0; + } else if (u2_k <= lowerbound) { + weights[k] = 1; + } else { + weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k ) - mu; + } + } + } + return weights; default: throw std::runtime_error( "GncOptimizer::calculateWeights: called with unknown loss type."); diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5006aa941b..3f784b96e1 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -162,7 +162,9 @@ TEST(GncOptimizer, checkMuConvergence) { gncParams); double mu = 1.0; - CHECK(gnc.checkMuConvergence(mu)); + CHECK(gnc.checkMuConvergence(mu, 0)); + + // TODO: test relative mu convergence } /* ************************************************************************* */ From d85d9c6194623a462c5b0288967503be2a95053f Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 13:45:12 -0500 Subject: [PATCH 25/45] minor fix --- 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 b6e6933ec7..40ed8c49a2 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GncOptimizer.cpp + * @file GncOptimizer.h * @brief The GncOptimizer class * @author Jingnan Shi * @author Luca Carlone From 58e49fc0fc64a207154221d7ddd30d9dc19ebaf6 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 15:08:50 -0500 Subject: [PATCH 26/45] fix scoping --- gtsam/nonlinear/GncOptimizer.h | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 40ed8c49a2..f5412c6ce1 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -340,7 +340,7 @@ class GncOptimizer { // update weights of known inlier/outlier measurements switch (params_.lossType) { - case GncParameters::GM: // use eq (12) in GNC paper + case GncParameters::GM: { // use eq (12) in GNC paper for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual @@ -349,22 +349,25 @@ class GncOptimizer { } } return weights; - case GncParameters::TLS: // use eq (14) in GNC paper + } + case GncParameters::TLS: { // use eq (14) in GNC paper double upperbound = (mu + 1) / mu * params_.barcSq; - double lowerbound = mu / (mu +1 ) * params_.barcSq; + double lowerbound = mu / (mu + 1) * params_.barcSq; for (size_t k : unknownWeights) { if (nfg_[k]) { - double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound ) { + double u2_k = nfg_[k]->error( + currentEstimate); // squared (and whitened) residual + if (u2_k >= upperbound) { weights[k] = 0; } else if (u2_k <= lowerbound) { weights[k] = 1; } else { - weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k ) - mu; + weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k) - mu; } } } return weights; + } default: throw std::runtime_error( "GncOptimizer::calculateWeights: called with unknown loss type."); From d0a81f8441ba91eab1bbfdf7af1fa0d69db99c54 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 16:04:36 -0500 Subject: [PATCH 27/45] mu initialization test & minor formatting fixes --- tests/testGncOptimizer.cpp | 133 +++++++++++++++++++++++-------------- 1 file changed, 83 insertions(+), 50 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 3f784b96e1..ca40231c92 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -16,29 +16,32 @@ * @author Luca Carlone * @author Frank Dellaert * - * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: - * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated + * Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to + * Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: + * https://arxiv.org/pdf/1909.08605.pdf) * * See also: - * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", - * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, + * Minimally-Tuned Algorithms, and Applications", arxiv: + * https://arxiv.org/pdf/2007.15109.pdf, 2020. */ -#include +#include #include +#include #include -#include using namespace std; using namespace gtsam; -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; static double tol = 1e-7; /* ************************************************************************* */ TEST(GncOptimizer, gncParamsConstructor) { - //check params are correctly parsed + // check params are correctly parsed LevenbergMarquardtParams lmParams; GncParams gncParams1(lmParams); CHECK(lmParams.equals(gncParams1.baseOptimizerParams)); @@ -69,7 +72,8 @@ TEST(GncOptimizer, gncParamsConstructor) { /* ************************************************************************* */ TEST(GncOptimizer, gncConstructor) { // has to have Gaussian noise models ! - auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point + auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor + // on a 2D point Point2 p0(3, 3); Values initial; @@ -77,8 +81,8 @@ TEST(GncOptimizer, gncConstructor) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(fg, initial, - gncParams); + auto gnc = + GncOptimizer>(fg, initial, gncParams); CHECK(gnc.getFactors().equals(fg)); CHECK(gnc.getState().equals(initial)); @@ -97,10 +101,11 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(fg_robust, - initial, gncParams); + auto gnc = GncOptimizer>( + fg_robust, initial, gncParams); - // make sure that when parsing the graph is transformed into one without robust loss + // make sure that when parsing the graph is transformed into one without + // robust loss CHECK(fg.equals(gnc.getFactors())); } @@ -112,13 +117,25 @@ TEST(GncOptimizer, initializeMu) { Values initial; initial.insert(X(1), p0); + // testing GM mu initialization LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); gncParams.setLossType( GncParams::RobustLossType::GM); - auto gnc = GncOptimizer>(fg, initial, - gncParams); - EXPECT_DOUBLES_EQUAL(gnc.initializeMu(), 2 * 198.999, 1e-3); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq (barcSq=1 in this example) + auto gnc_gm = + GncOptimizer>(fg, initial, gncParams); + // 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); + + // testing TLS mu initialization + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc_tls = + GncOptimizer>(fg, initial, gncParams); + // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) + // (barcSq=1 in this example) + EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3); } /* ************************************************************************* */ @@ -134,8 +151,8 @@ TEST(GncOptimizer, updateMu) { GncParams gncParams(lmParams); gncParams.setLossType( GncParams::RobustLossType::GM); - auto gnc = GncOptimizer>(fg, initial, - gncParams); + auto gnc = + GncOptimizer>(fg, initial, gncParams); double mu = 5.0; EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol); @@ -158,8 +175,8 @@ TEST(GncOptimizer, checkMuConvergence) { GncParams gncParams(lmParams); gncParams.setLossType( GncParams::RobustLossType::GM); - auto gnc = GncOptimizer>(fg, initial, - gncParams); + auto gnc = + GncOptimizer>(fg, initial, gncParams); double mu = 1.0; CHECK(gnc.checkMuConvergence(mu, 0)); @@ -175,11 +192,12 @@ TEST(GncOptimizer, calculateWeights) { Values initial; initial.insert(X(1), p0); - // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) + // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * + // 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) Vector weights_expected = Vector::Zero(4); - weights_expected[0] = 1.0; // zero error - weights_expected[1] = 1.0; // zero error - weights_expected[2] = 1.0; // zero error + weights_expected[0] = 1.0; // zero error + weights_expected[1] = 1.0; // zero error + weights_expected[2] = 1.0; // zero error weights_expected[3] = std::pow(1.0 / (50.0 + 1.0), 2); // outlier, error = 50 GaussNewtonParams gnParams; @@ -191,10 +209,11 @@ TEST(GncOptimizer, calculateWeights) { mu = 2.0; double barcSq = 5.0; - weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 + weights_expected[3] = + std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 gncParams.setInlierThreshold(barcSq); - auto gnc2 = GncOptimizer>(fg, initial, - gncParams); + auto gnc2 = + GncOptimizer>(fg, initial, gncParams); weights_actual = gnc2.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); } @@ -203,16 +222,17 @@ TEST(GncOptimizer, calculateWeights) { TEST(GncOptimizer, makeWeightedGraph) { // create original factor double sigma1 = 0.1; - NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma( - sigma1); + NonlinearFactorGraph nfg = + example::nonlinearFactorGraphWithGivenSigma(sigma1); // create expected double sigma2 = 10; - NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma( - sigma2); + NonlinearFactorGraph expected = + example::nonlinearFactorGraphWithGivenSigma(sigma2); // create weights - Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 + Vector weights = Vector::Ones( + 1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 weights[0] = 1e-4; // create actual @@ -223,7 +243,7 @@ TEST(GncOptimizer, makeWeightedGraph) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); auto gnc = GncOptimizer>(nfg, initial, - gncParams); + gncParams); NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights); // check it's all good @@ -240,8 +260,8 @@ TEST(GncOptimizer, optimizeSimple) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = GncOptimizer>(fg, initial, - gncParams); + auto gnc = + GncOptimizer>(fg, initial, gncParams); Values actual = gnc.optimize(); DOUBLES_EQUAL(0, fg.error(actual), tol); @@ -259,17 +279,23 @@ TEST(GncOptimizer, optimize) { GaussNewtonParams gnParams; GaussNewtonOptimizer gn(fg, initial, gnParams); Values gn_results = gn.optimize(); - // converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0) + // converges to incorrect point due to lack of robustness to an outlier, ideal + // solution is Point2(0,0) CHECK(assert_equal(Point2(0.25, 0.0), gn_results.at(X(1)), 1e-3)); // try with robust loss function and standard GN - auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses + auto fg_robust = + example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with + // factors wrapped in + // Geman McClure losses GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); Values gn2_results = gn2.optimize(); // converges to incorrect point, this time due to the nonconvexity of the loss - CHECK(assert_equal(Point2(0.999706, 0.0), gn2_results.at(X(1)), 1e-3)); + CHECK( + assert_equal(Point2(0.999706, 0.0), gn2_results.at(X(1)), 1e-3)); - // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity + // .. but graduated nonconvexity ensures both robustness and convergence in + // the face of nonconvexity GncParams gncParams(gnParams); // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -315,31 +341,38 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { boost::tie(graph, initial) = load2D(filename); // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); - graph -> addPrior(0, priorMean, priorNoise); + SharedDiagonal priorNoise = + noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); + graph->addPrior(0, priorMean, priorNoise); /// get expected values by optimizing outlier-free graph Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); // add a few outliers - SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01)); - graph->push_back( BetweenFactor(90 , 50 , Pose2(), betweenNoise) ); // some arbitrary and incorrect between factor + SharedDiagonal betweenNoise = + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01)); + graph->push_back(BetweenFactor( + 90, 50, Pose2(), + betweenNoise)); // some arbitrary and incorrect between factor /// get expected values by optimizing outlier-free graph - Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + Values expectedWithOutliers = + LevenbergMarquardtOptimizer(*graph, *initial).optimize(); // as expected, the following test fails due to the presence of an outlier! // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); // GNC - // Note: in difficult instances, we set the odometry measurements to be inliers, - // but this problem is simple enought to succeed even without that assumption - // std::vector knownInliers; + // Note: in difficult instances, we set the odometry measurements to be + // inliers, but this problem is simple enought to succeed even without that + // assumption std::vector knownInliers; GncParams gncParams; - auto gnc = GncOptimizer>(*graph, *initial, gncParams); + auto gnc = + GncOptimizer>(*graph, *initial, gncParams); Values actual = gnc.optimize(); // compare - CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! + CHECK( + assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! } /* ************************************************************************* */ From 9caa0d14cf99fc48b46bbf61da9224d5e7056e1f Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 16:16:21 -0500 Subject: [PATCH 28/45] mu update test Separated GM & TLS case make sure the mu set size is explicitly stated (does not depend on default values) --- tests/testGncOptimizer.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index ca40231c92..a1c6fe5262 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -139,7 +139,7 @@ TEST(GncOptimizer, initializeMu) { } /* ************************************************************************* */ -TEST(GncOptimizer, updateMu) { +TEST(GncOptimizer, updateMuGM) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -151,6 +151,7 @@ TEST(GncOptimizer, updateMu) { GncParams gncParams(lmParams); gncParams.setLossType( GncParams::RobustLossType::GM); + gncParams.setMuStep(1.4); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -162,6 +163,27 @@ TEST(GncOptimizer, updateMu) { EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), 1.0, tol); } +/* ************************************************************************* */ +TEST(GncOptimizer, updateMuTLS) { + // 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.setMuStep(1.4); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double mu = 5.0; + EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu * 1.4, tol); +} + /* ************************************************************************* */ TEST(GncOptimizer, checkMuConvergence) { // has to have Gaussian noise models ! From 428d17a4bc30d1193be3b37cfd71cae8fd4ec4da Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 16:35:46 -0500 Subject: [PATCH 29/45] correctly check relative difference between mu valus at consecutive iterations --- gtsam/nonlinear/GncOptimizer.h | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index f5412c6ce1..8114877793 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -203,9 +203,11 @@ class GncOptimizer { double mu = initializeMu(); double mu_prev = mu; - // handle the degenerate case for TLS cost that corresponds to small - // maximum residual error at initialization - if (mu <= 0 && params_.lossType == GncParameters::TLS) { + // handle the degenerate case that corresponds to small + // maximum residual errors at initialization + // For GM: if residual error is small, mu -> 0 + // For TLS: if residual error is small, mu -> -1 + if (mu <= 0) { if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " "initialization is small." << std::endl; @@ -230,6 +232,10 @@ class GncOptimizer { GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); result = baseOptimizer_iter.optimize(); + // update mu + mu_prev = mu; + mu = updateMu(mu); + // stopping condition if (checkMuConvergence(mu, mu_prev)) { // display info @@ -240,9 +246,6 @@ class GncOptimizer { } break; } - // otherwise update mu - mu_prev = mu; - mu = updateMu(mu); } return result; } From 594f63d1f694c9b13373f580ec5de66fe7b62d91 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 17:28:35 -0500 Subject: [PATCH 30/45] test fix --- tests/testGncOptimizer.cpp | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a1c6fe5262..5734dfc43f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -135,7 +135,7 @@ TEST(GncOptimizer, initializeMu) { GncOptimizer>(fg, initial, gncParams); // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) // (barcSq=1 in this example) - EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3); + EXPECT_DOUBLES_EQUAL(gnc_tls.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3); } /* ************************************************************************* */ @@ -185,7 +185,7 @@ TEST(GncOptimizer, updateMuTLS) { } /* ************************************************************************* */ -TEST(GncOptimizer, checkMuConvergence) { +TEST(GncOptimizer, checkMuConvergenceGM) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -202,8 +202,26 @@ TEST(GncOptimizer, checkMuConvergence) { double mu = 1.0; CHECK(gnc.checkMuConvergence(mu, 0)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkMuConvergenceTLS) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); - // TODO: test relative mu convergence + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double mu = 1.0; + CHECK(gnc.checkMuConvergence(mu, mu)); } /* ************************************************************************* */ From 398c01375ef41ea79aa1486b099a03e5843bcf2c Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Mon, 7 Dec 2020 20:20:51 -0500 Subject: [PATCH 31/45] more unit tests --- tests/testGncOptimizer.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5734dfc43f..b3bef11e09 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -225,7 +225,7 @@ TEST(GncOptimizer, checkMuConvergenceTLS) { } /* ************************************************************************* */ -TEST(GncOptimizer, calculateWeights) { +TEST(GncOptimizer, calculateWeightsGM) { auto fg = example::sharedNonRobustFactorGraphWithOutliers(); Point2 p0(0, 0); @@ -242,6 +242,8 @@ TEST(GncOptimizer, calculateWeights) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -258,6 +260,31 @@ TEST(GncOptimizer, calculateWeights) { CHECK(assert_equal(weights_expected, weights_actual, tol)); } +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeightsTLS) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(0, 0); + Values initial; + initial.insert(X(1), p0); + + // we have 4 factors, 3 with zero errors (inliers), 1 with error + Vector weights_expected = Vector::Zero(4); + weights_expected[0] = 1.0; // zero error + weights_expected[1] = 1.0; // zero error + weights_expected[2] = 1.0; // zero error + weights_expected[3] = 0; // outliers + + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, gncParams); + double mu = 1.0; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); +} + /* ************************************************************************* */ TEST(GncOptimizer, makeWeightedGraph) { // create original factor From 75bd3dc52cb48126f84df1f7822f5b46278b8a69 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 21 Dec 2020 20:32:21 -0500 Subject: [PATCH 32/45] templating on params is still problematic --- gtsam/nonlinear/GaussNewtonOptimizer.h | 2 ++ gtsam/nonlinear/GncOptimizer.h | 14 ++++++++++---- gtsam/nonlinear/LevenbergMarquardtParams.h | 3 +++ 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index a436597e58..3ef4dfdeb6 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -28,6 +28,8 @@ class GaussNewtonOptimizer; * NonlinearOptimizationParams. */ class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams { +public: + typedef GaussNewtonOptimizer OptimizerType; }; /** diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 8114877793..730c1aa436 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -36,6 +36,9 @@ namespace gtsam { template class GncParams { public: + //typedef BaseOptimizerParameters::OptimizerType GncOptimizerType; + typedef BaseOptimizerParameters::OptimizerType OptimizerType; + /** Verbosity levels */ enum VerbosityGNC { SILENT = 0, SUMMARY, VALUES @@ -46,8 +49,6 @@ class GncParams { GM /*Geman McClure*/, TLS /*Truncated least squares*/ }; - using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; - /// Constructor GncParams(const BaseOptimizerParameters& baseOptimizerParams) : baseOptimizerParams(baseOptimizerParams) { @@ -145,6 +146,11 @@ template class GncOptimizer { public: // types etc +// typedef BaseOptimizerParameters::OptimizerType GncOptimizerType; + // typedef GncParameters::BaseOptimizerParameters::OptimizerType BaseOptimizer; // + //typedef BaseOptimizerParameters::OptimizerType BaseOptimizer; + //typedef GaussNewtonOptimizer BaseOptimizer; + typedef GncParameters::OptimizerType BaseOptimizer; private: NonlinearFactorGraph nfg_; @@ -198,7 +204,7 @@ class GncOptimizer { Values optimize() { // start by assuming all measurements are inliers weights_ = Vector::Ones(nfg_.size()); - GaussNewtonOptimizer baseOptimizer(nfg_, state_); + BaseOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); double mu_prev = mu; @@ -229,7 +235,7 @@ class GncOptimizer { // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); - GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); + BaseOptimizer baseOptimizer_iter(graph_iter, state_); result = baseOptimizer_iter.optimize(); // update mu diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index 4962ad3660..20a8eb4c11 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -25,6 +25,8 @@ namespace gtsam { +class LevenbergMarquardtOptimizer; + /** Parameters for Levenberg-Marquardt optimization. Note that this parameters * class inherits from NonlinearOptimizerParams, which specifies the parameters * common to all nonlinear optimization algorithms. This class also contains @@ -40,6 +42,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { static VerbosityLM verbosityLMTranslator(const std::string &s); static std::string verbosityLMTranslator(VerbosityLM value); + typedef LevenbergMarquardtOptimizer OptimizerType; public: From 7efd5cc3681816d0977ac6e59b054aaeed99cfca Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 21 Dec 2020 21:06:25 -0500 Subject: [PATCH 33/45] finally fixed the typedef --- gtsam/nonlinear/GncOptimizer.h | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 730c1aa436..dd6c8d17d5 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -36,8 +36,8 @@ namespace gtsam { template class GncParams { public: - //typedef BaseOptimizerParameters::OptimizerType GncOptimizerType; - typedef BaseOptimizerParameters::OptimizerType OptimizerType; + /** For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer */ + typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; /** Verbosity levels */ enum VerbosityGNC { @@ -145,12 +145,8 @@ class GncParams { template class GncOptimizer { public: - // types etc -// typedef BaseOptimizerParameters::OptimizerType GncOptimizerType; - // typedef GncParameters::BaseOptimizerParameters::OptimizerType BaseOptimizer; // - //typedef BaseOptimizerParameters::OptimizerType BaseOptimizer; - //typedef GaussNewtonOptimizer BaseOptimizer; - typedef GncParameters::OptimizerType BaseOptimizer; + /** For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer */ + typedef typename GncParameters::OptimizerType BaseOptimizer; private: NonlinearFactorGraph nfg_; From 0e09f019ef14265f83b5a91348807280fcfc634a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 21 Dec 2020 22:28:07 -0500 Subject: [PATCH 34/45] fixed templating, added a strict unit test on inlier threshold --- gtsam/nonlinear/GncOptimizer.h | 12 +++--- tests/testGncOptimizer.cpp | 70 ++++++++++++++++++++++++++++++++++ 2 files changed, 77 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index dd6c8d17d5..d4b25409fe 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -264,11 +264,12 @@ class GncOptimizer { // set initial mu switch (params_.lossType) { case GncParameters::GM: + // surrogate cost is convex for large mu return 2 * rmax_sq / params_.barcSq; // initial mu case GncParameters::TLS: - // initialize mu to the value specified in Remark 5 in GNC paper - // degenerate case: residual is close to zero so mu approximately equals - // to -1 + // 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); default: throw std::runtime_error( @@ -280,9 +281,10 @@ class GncOptimizer { 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 + // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) + return std::max(1.0, mu / params_.muStep); case GncParameters::TLS: - // increases mu at each iteration + // increases mu at each iteration (original cost is recovered for mu -> inf) return mu * params_.muStep; default: throw std::runtime_error( diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index b3bef11e09..85a196bd74 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -285,6 +286,75 @@ TEST(GncOptimizer, calculateWeightsTLS) { CHECK(assert_equal(weights_expected, weights_actual, tol)); } +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeightsTLS2) { + + // create values + Point2 x_val(0.0, 0.0); + Point2 x_prior(1.0, 0.0); + Values initial; + initial.insert(X(1), x_val); + + // create very simple factor graph with a single factor 0.5 * 1/sigma^2 * || x - [1;0] ||^2 + double sigma = 1; + SharedDiagonal noise = + noiseModel::Diagonal::Sigmas(Vector2(sigma, sigma)); + NonlinearFactorGraph nfg; + nfg.add(PriorFactor(X(1),x_prior,noise)); + + // cost of the factor: + DOUBLES_EQUAL(0.5 * 1/(sigma*sigma), nfg.error(initial), tol); + + // check the TLS weights are correct: CASE 1: residual below barcsq + { + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 1.0; // inlier + // actual: + GaussNewtonParams gnParams; + 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 + auto gnc = GncOptimizer>(nfg, initial, gncParams); + double mu = 1e6; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); + } + // check the TLS weights are correct: CASE 2: residual above barcsq + { + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 0.0; // outlier + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + gncParams.setInlierThreshold(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); + CHECK(assert_equal(weights_expected, weights_actual, tol)); + } + // check the TLS weights are correct: CASE 2: residual at barcsq + { + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 0.5; // undecided + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + gncParams.setInlierThreshold(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); + CHECK(assert_equal(weights_expected, weights_actual, 1e-5)); + } +} + /* ************************************************************************* */ TEST(GncOptimizer, makeWeightedGraph) { // create original factor From cd82a56214b931884e83e19a0182252add9b7687 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 21 Dec 2020 22:32:34 -0500 Subject: [PATCH 35/45] 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 d4b25409fe..df87c44330 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -83,8 +83,11 @@ class GncParams { } /** 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 85a196bd74..7c4fbd5fa7 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); From 046db8749e985144ece86d44a42dc932b41528e6 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Tue, 22 Dec 2020 13:40:52 -0500 Subject: [PATCH 36/45] Fix TLS convergence check --- gtsam/nonlinear/GncOptimizer.h | 59 +++++++++++++++++++++++++++------- tests/testGncOptimizer.cpp | 4 +-- 2 files changed, 50 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index df87c44330..0f4d5ea275 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -66,7 +66,7 @@ class GncParams { 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 relativeMuTol = 1e-5; ///< The maximum relative mu decrease to stop iterating + double relativeCostTol = 1e-5; ///< The maximum relative cost change to stop iterating VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ @@ -95,8 +95,7 @@ class GncParams { muStep = step; } /// Set the maximum relative difference in mu values to stop iterating - void setRelativeMuTol(double value) { - relativeMuTol = value; + void setRelativeMuTol(double value) { relativeCostTol = value; } /// Set the verbosity level void setVerbosityGNC(const VerbosityGNC verbosity) { @@ -206,7 +205,8 @@ class GncOptimizer { BaseOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); - double mu_prev = mu; + double cost = calculateWeightedCost(); + double prev_cost = cost; // handle the degenerate case that corresponds to small // maximum residual errors at initialization @@ -232,17 +232,17 @@ class GncOptimizer { // 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(); - // update mu - mu_prev = mu; - mu = updateMu(mu); - // stopping condition - if (checkMuConvergence(mu, mu_prev)) { + if (checkConvergence(mu, cost, prev_cost)) { // display info if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; @@ -251,6 +251,9 @@ class GncOptimizer { } break; } + + // update mu + mu = updateMu(mu); } return result; } @@ -295,19 +298,53 @@ class GncOptimizer { } } + /// 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 double mu_prev) const { + 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."); + } + } + + /// 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."); + } + } + + /// check for convergence between consecutive GNC iterations + bool checkConvergence(const double mu, + const double cost, + const double prev_cost) const { + switch (params_.lossType) { + case GncParameters::GM: + return checkMuConvergence(mu); case GncParameters::TLS: - return std::fabs(mu - mu_prev) < params_.relativeMuTol; + return checkCostConvergence(cost, prev_cost); 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 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 7c4fbd5fa7..a0e3714637 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -202,7 +202,7 @@ TEST(GncOptimizer, checkMuConvergenceGM) { GncOptimizer>(fg, initial, gncParams); double mu = 1.0; - CHECK(gnc.checkMuConvergence(mu, 0)); + CHECK(gnc.checkMuConvergence(mu)); } /* ************************************************************************* */ @@ -222,7 +222,7 @@ TEST(GncOptimizer, checkMuConvergenceTLS) { GncOptimizer>(fg, initial, gncParams); double mu = 1.0; - CHECK(gnc.checkMuConvergence(mu, mu)); + CHECK(gnc.checkMuConvergence(mu)); } /* ************************************************************************* */ From be5d3d234382f32da17b4aed14432baa7d8c085d Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Tue, 22 Dec 2020 20:28:10 -0500 Subject: [PATCH 37/45] update function name --- 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 0f4d5ea275..d47b6bc824 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -95,7 +95,7 @@ class GncParams { muStep = step; } /// Set the maximum relative difference in mu values to stop iterating - void setRelativeMuTol(double value) { relativeCostTol = value; + void setRelativeCostTol(double value) { relativeCostTol = value; } /// Set the verbosity level void setVerbosityGNC(const VerbosityGNC verbosity) { From c57174436f31662f0154fc241d05b9bbe5cce49a Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Tue, 22 Dec 2020 21:08:42 -0500 Subject: [PATCH 38/45] fix test --- tests/testGncOptimizer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a0e3714637..dc96762b8c 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -206,7 +206,7 @@ TEST(GncOptimizer, checkMuConvergenceGM) { } /* ************************************************************************* */ -TEST(GncOptimizer, checkMuConvergenceTLS) { +TEST(GncOptimizer, checkConvergenceTLS) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -216,13 +216,14 @@ TEST(GncOptimizer, checkMuConvergenceTLS) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); + gncParams.setRelativeCostTol(1e-5); gncParams.setLossType( GncParams::RobustLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); - double mu = 1.0; - CHECK(gnc.checkMuConvergence(mu)); + CHECK(gnc.checkCostConvergence(1.0, 1.0)); + CHECK(!gnc.checkCostConvergence(1.0, 2.0)); } /* ************************************************************************* */ From dc5c769e7ca4a9cd6d23a1def4d4a95bfa32a93e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 23 Dec 2020 22:08:44 -0500 Subject: [PATCH 39/45] - 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 d47b6bc824..b66e0f5230 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -62,11 +62,12 @@ class GncParams { /// 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 @@ class GncParams { /// 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 @@ class GncParams { 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 @@ class GncOptimizer { 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 @@ class GncOptimizer { 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 @@ class GncOptimizer { // 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 @@ class GncOptimizer { // 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 @@ class GncOptimizer { } } - /// 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 dc96762b8c..9036fc97f0 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,29 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { knownInliers.push_back(2); // nonconvexity with known inliers + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType( + GncParams::RobustLossType::GM); + //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); + } + { GncParams gncParams; gncParams.setKnownInliers(knownInliers); - // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); Values gnc_result = gnc.optimize(); @@ -468,6 +595,28 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { 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); + } } /* ************************************************************************* */ From 92ed225a557cfec8b58f1038ed8c6cb5b066adf7 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Thu, 24 Dec 2020 11:19:47 -0500 Subject: [PATCH 40/45] minor fixes --- gtsam/nonlinear/GncOptimizer.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index b66e0f5230..6637e33d4f 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -66,7 +66,7 @@ class GncParams { 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 if below this threshold, stop iterating + 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) VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ @@ -111,7 +111,7 @@ class GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures * */ - void setKnownInliers(const std::vector knownIn) { + void setKnownInliers(const std::vector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++) knownInliers.push_back(knownIn[i]); } @@ -338,7 +338,7 @@ class GncOptimizer { /// check convergence of relative cost differences bool checkCostConvergence(const double cost, const double prev_cost) const { - bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost,1e-7) < params_.relativeCostTol; + 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; @@ -406,7 +406,7 @@ class GncOptimizer { } /// calculate gnc weights - Vector calculateWeights(const Values currentEstimate, const double mu) { + Vector calculateWeights(const Values& currentEstimate, const double mu) { Vector weights = Vector::Ones(nfg_.size()); // do not update the weights that the user has decided are known inliers From 06dfeb7ac5f5323f525263855803964d330f6d80 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 28 Dec 2020 20:43:35 -0500 Subject: [PATCH 41/45] moved GncParams to separate file, addressing comments by Frank, 1/n --- gtsam/nonlinear/GaussNewtonOptimizer.h | 2 +- gtsam/nonlinear/GncOptimizer.h | 136 ++----------------- gtsam/nonlinear/GncParams.h | 151 +++++++++++++++++++++ gtsam/nonlinear/LevenbergMarquardtParams.h | 2 +- tests/testGncOptimizer.cpp | 50 +++---- 5 files changed, 180 insertions(+), 161 deletions(-) create mode 100644 gtsam/nonlinear/GncParams.h diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 3ef4dfdeb6..a3923524b7 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -29,7 +29,7 @@ class GaussNewtonOptimizer; */ class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams { public: - typedef GaussNewtonOptimizer OptimizerType; + using OptimizerType = GaussNewtonOptimizer; }; /** diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 6637e33d4f..f283945452 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -26,129 +26,11 @@ #pragma once -#include -#include +#include #include namespace gtsam { -/* ************************************************************************* */ -template -class GncParams { -public: - /** For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer */ - typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; - - /** Verbosity levels */ - enum VerbosityGNC { - SILENT = 0, SUMMARY, VALUES - }; - - /** Choice of robust loss function for GNC */ - enum RobustLossType { - GM /*Geman McClure*/, TLS /*Truncated least squares*/ - }; - - /// Constructor - GncParams(const BaseOptimizerParameters& baseOptimizerParams) : - baseOptimizerParams(baseOptimizerParams) { - } - - /// Default constructor - GncParams() : - baseOptimizerParams() { - } - - /// 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 = 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) - VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ - std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ - - /// Set the robust loss function to be used in GNC (chosen among the ones in RobustLossType) - void setLossType(const RobustLossType type) { - lossType = type; - } - /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended) - void setMaxIterations(const size_t maxIter) { - std::cout - << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " - << std::endl; - 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; - } - /// 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; - } - /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector - * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, - * and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. - * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and - * only apply GNC to prune outliers from the loop closures - * */ - void setKnownInliers(const std::vector& knownIn) { - for (size_t i = 0; i < knownIn.size(); i++) - knownInliers.push_back(knownIn[i]); - } - /// 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 - && verbosityGNC == other.verbosityGNC - && knownInliers == other.knownInliers; - } - /// print function - void print(const std::string& str) const { - std::cout << str << "\n"; - switch (lossType) { - case GM: - std::cout << "lossType: Geman McClure" << "\n"; - break; - case TLS: - std::cout << "lossType: Truncated Least-squares" << "\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"; - 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"; - baseOptimizerParams.print(str); - } -}; - /* ************************************************************************* */ template class GncOptimizer { @@ -219,11 +101,11 @@ class GncOptimizer { // For GM: if residual error is small, mu -> 0 // For TLS: if residual error is small, mu -> -1 if (mu <= 0) { - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { + if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " "initialization is small." << std::endl; } - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); std::cout << "mu: " << mu << std::endl; } @@ -234,7 +116,7 @@ class GncOptimizer { for (iter = 0; iter < params_.maxIterations; iter++) { // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { std::cout << "iter: " << iter << std::endl; result.print("result\n"); std::cout << "mu: " << mu << std::endl; @@ -259,13 +141,13 @@ class GncOptimizer { prev_cost = cost; // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } } // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { + if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; std::cout << "final weights: " << weights_ << std::endl; @@ -331,7 +213,7 @@ class GncOptimizer { throw std::runtime_error( "GncOptimizer::checkMuConvergence: called with unknown loss type."); } - if (muConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) std::cout << "muConverged = true " << std::endl; return muConverged; } @@ -339,7 +221,7 @@ class GncOptimizer { /// check convergence of relative cost differences bool checkCostConvergence(const double cost, const double prev_cost) const { bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost,1e-7) < params_.relativeCostTol; - if (costConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) std::cout << "checkCostConvergence = true " << std::endl; return costConverged; } @@ -364,7 +246,7 @@ class GncOptimizer { throw std::runtime_error( "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); } - if (weightsConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + if (weightsConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) std::cout << "weightsConverged = true " << std::endl; return weightsConverged; } diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h new file mode 100644 index 0000000000..98fb19f5fe --- /dev/null +++ b/gtsam/nonlinear/GncParams.h @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GncOptimizer.h + * @brief The GncOptimizer class + * @author Jingnan Shi + * @author Luca Carlone + * @author Frank Dellaert + * + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: + * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * + * See also: + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", + * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +class GncParams { +public: + /** For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer */ + typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; + + /** Verbosity levels */ + enum Verbosity { + SILENT = 0, SUMMARY, VALUES + }; + + /** Choice of robust loss function for GNC */ + enum RobustLossType { + GM /*Geman McClure*/, TLS /*Truncated least squares*/ + }; + + /// Constructor + GncParams(const BaseOptimizerParameters& baseOptimizerParams) : + baseOptimizerParams(baseOptimizerParams) { + } + + /// Default constructor + GncParams() : + baseOptimizerParams() { + } + + /// 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 = 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) + Verbosity verbosity = SILENT; /* verbosity level */ + std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ + + /// Set the robust loss function to be used in GNC (chosen among the ones in RobustLossType) + void setLossType(const RobustLossType type) { + lossType = type; + } + /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended) + void setMaxIterations(const size_t maxIter) { + std::cout + << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " + << std::endl; + 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; + } + /// 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 Verbosity verbosity) { + verbosity = verbosity; + } + /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector + * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, + * and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. + * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and + * only apply GNC to prune outliers from the loop closures + * */ + void setKnownInliers(const std::vector& knownIn) { + for (size_t i = 0; i < knownIn.size(); i++) + knownInliers.push_back(knownIn[i]); + } + /// 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 + && verbosity == other.verbosity + && knownInliers == other.knownInliers; + } + /// print function + void print(const std::string& str) const { + std::cout << str << "\n"; + switch (lossType) { + case GM: + std::cout << "lossType: Geman McClure" << "\n"; + break; + case TLS: + std::cout << "lossType: Truncated Least-squares" << "\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"; + std::cout << "relativeCostTol: " << relativeCostTol << "\n"; + std::cout << "weightsTol: " << weightsTol << "\n"; + std::cout << "verbosity: " << verbosity << "\n"; + for (size_t i = 0; i < knownInliers.size(); i++) + std::cout << "knownInliers: " << knownInliers[i] << "\n"; + baseOptimizerParams.print(str); + } +}; + +} diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index 20a8eb4c11..30e783fc99 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -42,7 +42,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { static VerbosityLM verbosityLMTranslator(const std::string &s); static std::string verbosityLMTranslator(VerbosityLM value); - typedef LevenbergMarquardtOptimizer OptimizerType; + using OptimizerType = LevenbergMarquardtOptimizer; public: diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 9036fc97f0..f8e12fcd3e 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -80,8 +80,7 @@ TEST(GncOptimizer, gncConstructor) { Values initial; initial.insert(X(1), p0); - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -100,8 +99,7 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { Values initial; initial.insert(X(1), p0); - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; auto gnc = GncOptimizer>( fg_robust, initial, gncParams); @@ -119,8 +117,7 @@ TEST(GncOptimizer, initializeMu) { initial.insert(X(1), p0); // testing GM mu initialization - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::GM); auto gnc_gm = @@ -148,8 +145,7 @@ TEST(GncOptimizer, updateMuGM) { Values initial; initial.insert(X(1), p0); - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::GM); gncParams.setMuStep(1.4); @@ -173,8 +169,7 @@ TEST(GncOptimizer, updateMuTLS) { Values initial; initial.insert(X(1), p0); - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setMuStep(1.4); gncParams.setLossType( GncParams::RobustLossType::TLS); @@ -195,8 +190,7 @@ TEST(GncOptimizer, checkMuConvergence) { initial.insert(X(1), p0); { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::GM); auto gnc = @@ -206,8 +200,7 @@ TEST(GncOptimizer, checkMuConvergence) { CHECK(gnc.checkMuConvergence(mu)); } { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::TLS); auto gnc = @@ -228,8 +221,7 @@ TEST(GncOptimizer, checkCostConvergence) { initial.insert(X(1), p0); { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setRelativeCostTol(0.49); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -240,8 +232,7 @@ TEST(GncOptimizer, checkCostConvergence) { CHECK(!gnc.checkCostConvergence(cost, prev_cost)); } { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setRelativeCostTol(0.51); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -263,8 +254,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { initial.insert(X(1), p0); { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::GM); auto gnc = @@ -274,8 +264,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM } { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::TLS); auto gnc = @@ -286,8 +275,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { CHECK(gnc.checkWeightsConvergence(weights)); } { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::TLS); auto gnc = @@ -298,8 +286,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { CHECK(!gnc.checkWeightsConvergence(weights)); } { - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setLossType( GncParams::RobustLossType::TLS); gncParams.setWeightsTol(0.1); @@ -321,8 +308,7 @@ TEST(GncOptimizer, checkConvergenceTLS) { Values initial; initial.insert(X(1), p0); - LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); + GncParams gncParams; gncParams.setRelativeCostTol(1e-5); gncParams.setLossType( GncParams::RobustLossType::TLS); @@ -542,7 +528,7 @@ TEST(GncOptimizer, optimize) { // .. but graduated nonconvexity ensures both robustness and convergence in // the face of nonconvexity GncParams gncParams(gnParams); - // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); + // gncParams.setVerbosityGNC(GncParams::Verbosity::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)); @@ -567,7 +553,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { gncParams.setKnownInliers(knownInliers); gncParams.setLossType( GncParams::RobustLossType::GM); - //gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); Values gnc_result = gnc.optimize(); @@ -584,7 +570,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { gncParams.setKnownInliers(knownInliers); gncParams.setLossType( GncParams::RobustLossType::TLS); - // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); + // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); Values gnc_result = gnc.optimize(); @@ -603,7 +589,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { gncParams.setKnownInliers(knownInliers); gncParams.setLossType( GncParams::RobustLossType::TLS); - //gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); gncParams.setInlierCostThreshold( 100.0 ); auto gnc = GncOptimizer>(fg, initial, gncParams); From eea52766d1220d69a276916f767527005945d3fe Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 28 Dec 2020 20:49:17 -0500 Subject: [PATCH 42/45] renamed enum --- gtsam/nonlinear/GncParams.h | 12 +++++------ tests/testGncOptimizer.cpp | 40 ++++++++++++++++++------------------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 98fb19f5fe..30ceef4c72 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -44,7 +44,7 @@ class GncParams { }; /** Choice of robust loss function for GNC */ - enum RobustLossType { + enum GncLossType { GM /*Geman McClure*/, TLS /*Truncated least squares*/ }; @@ -61,7 +61,7 @@ class GncParams { /// 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 = TLS; /* default loss*/ + 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 */ @@ -70,8 +70,8 @@ class GncParams { Verbosity verbosity = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ - /// Set the robust loss function to be used in GNC (chosen among the ones in RobustLossType) - void setLossType(const RobustLossType type) { + /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType) + void setLossType(const GncLossType type) { lossType = type; } /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended) @@ -101,8 +101,8 @@ class GncParams { void setWeightsTol(double value) { weightsTol = value; } /// Set the verbosity level - void setVerbosityGNC(const Verbosity verbosity) { - verbosity = verbosity; + void setVerbosityGNC(const Verbosity value) { + verbosity = value; } /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index f8e12fcd3e..15a83eb4b9 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::GM); + gncParams2c.setLossType(GncParams::GncLossType::GM); CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); } @@ -119,7 +119,7 @@ TEST(GncOptimizer, initializeMu) { // testing GM mu initialization GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::GM); + GncParams::GncLossType::GM); auto gnc_gm = GncOptimizer>(fg, initial, gncParams); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq @@ -128,7 +128,7 @@ TEST(GncOptimizer, initializeMu) { // testing TLS mu initialization gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc_tls = GncOptimizer>(fg, initial, gncParams); // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) @@ -147,7 +147,7 @@ TEST(GncOptimizer, updateMuGM) { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::GM); + GncParams::GncLossType::GM); gncParams.setMuStep(1.4); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -172,7 +172,7 @@ TEST(GncOptimizer, updateMuTLS) { GncParams gncParams; gncParams.setMuStep(1.4); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -192,7 +192,7 @@ TEST(GncOptimizer, checkMuConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::GM); + GncParams::GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -202,7 +202,7 @@ TEST(GncOptimizer, checkMuConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -256,7 +256,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::GM); + GncParams::GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -266,7 +266,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -277,7 +277,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -288,7 +288,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); gncParams.setWeightsTol(0.1); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -311,7 +311,7 @@ TEST(GncOptimizer, checkConvergenceTLS) { GncParams gncParams; gncParams.setRelativeCostTol(1e-5); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -338,7 +338,7 @@ TEST(GncOptimizer, calculateWeightsGM) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType( - GncParams::RobustLossType::GM); + GncParams::GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -373,7 +373,7 @@ TEST(GncOptimizer, calculateWeightsTLS) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -408,7 +408,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::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); double mu = 1e6; @@ -424,7 +424,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); 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 @@ -440,7 +440,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); 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 @@ -552,7 +552,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { GncParams gncParams; gncParams.setKnownInliers(knownInliers); gncParams.setLossType( - GncParams::RobustLossType::GM); + GncParams::GncLossType::GM); //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -569,7 +569,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { GncParams gncParams; gncParams.setKnownInliers(knownInliers); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -588,7 +588,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { GncParams gncParams; gncParams.setKnownInliers(knownInliers); gncParams.setLossType( - GncParams::RobustLossType::TLS); + GncParams::GncLossType::TLS); //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); gncParams.setInlierCostThreshold( 100.0 ); auto gnc = GncOptimizer>(fg, initial, gncParams); From dfdd2067081ec48422324f3703b1e9b9ca9d0e54 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 28 Dec 2020 21:03:20 -0500 Subject: [PATCH 43/45] addressed all except 2 comments by Frank. waiting for inputs on the 2 outstanding issues --- gtsam/nonlinear/GncOptimizer.h | 226 +++++++++--------- gtsam/nonlinear/GncParams.h | 92 ++++---- tests/testGncOptimizer.cpp | 412 ++++++++++++++++----------------- 3 files changed, 365 insertions(+), 365 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index f283945452..bbc3b9f849 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -34,21 +34,22 @@ namespace gtsam { /* ************************************************************************* */ template class GncOptimizer { -public: - /** For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer */ + public: + /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename GncParameters::OptimizerType BaseOptimizer; -private: - NonlinearFactorGraph nfg_; - Values state_; - GncParameters params_; - Vector weights_; // this could be a local variable in optimize, but it is useful to make it accessible from outside + private: + NonlinearFactorGraph nfg_; ///< Original factor graph to be solved by GNC. + 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). -public: - /// Constructor + public: + /// Constructor. GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const GncParameters& params = GncParameters()) : - state_(initialValues), params_(params) { + const GncParameters& params = GncParameters()) + : state_(initialValues), + params_(params) { // make sure all noiseModels are Gaussian or convert to Gaussian nfg_.resize(graph.size()); @@ -58,35 +59,39 @@ class GncOptimizer { NoiseModelFactor>(graph[i]); noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast< noiseModel::Robust>(factor->noiseModel()); - if (robust) { // if the factor has a robust loss, we have to change it: + if (robust) { // if the factor has a robust loss, we have to change it: SharedNoiseModel gaussianNoise = robust->noise(); - NoiseModelFactor::shared_ptr gaussianFactor = - factor->cloneWithNewNoiseModel(gaussianNoise); + NoiseModelFactor::shared_ptr gaussianFactor = factor + ->cloneWithNewNoiseModel(gaussianNoise); nfg_[i] = gaussianFactor; - } else { // else we directly push it back + } else { // else we directly push it back nfg_[i] = factor; } } } } - /// Access a copy of the internal factor graph + /// Access a copy of the internal factor graph. NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); } - /// Access a copy of the internal values + + /// Access a copy of the internal values. Values getState() const { return Values(state_); } - /// Access a copy of the parameters + + /// Access a copy of the parameters. GncParameters getParams() const { return GncParameters(params_); } - /// Access a copy of the GNC weights + + /// Access a copy of the GNC weights. Vector getWeights() const { return weights_; } - /// Compute optimal solution using graduated non-convexity + + /// Compute optimal solution using graduated non-convexity. Values optimize() { // start by assuming all measurements are inliers weights_ = Vector::Ones(nfg_.size()); @@ -94,7 +99,7 @@ class GncOptimizer { Values result = baseOptimizer.optimize(); double mu = initializeMu(); double prev_cost = nfg_.error(result); - double cost = 0.0; // this will be updated in the main loop + 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 @@ -103,7 +108,8 @@ class GncOptimizer { if (mu <= 0) { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " - "initialization is small." << std::endl; + "initialization is small." + << std::endl; } if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); @@ -132,7 +138,9 @@ class GncOptimizer { // stopping condition cost = graph_iter.error(result); - if (checkConvergence(mu, weights_, cost, prev_cost)) { break; } + if (checkConvergence(mu, weights_, cost, prev_cost)) { + break; + } // update mu mu = updateMu(mu); @@ -157,7 +165,7 @@ class GncOptimizer { return result; } - /// initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper) + /// 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; @@ -168,75 +176,80 @@ class GncOptimizer { } // set initial mu switch (params_.lossType) { - case GncParameters::GM: - // surrogate cost is convex for large mu - return 2 * rmax_sq / params_.barcSq; // initial mu - case GncParameters::TLS: - // 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) - // 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."); + case GncParameters::GM: + // surrogate cost is convex for large mu + return 2 * rmax_sq / params_.barcSq; // initial mu + case GncParameters::TLS: + /* 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) + 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."); } } - /// update the gnc parameter mu to gradually increase nonconvexity + /// Update the gnc parameter mu to gradually increase nonconvexity. double updateMu(const double mu) const { switch (params_.lossType) { - case GncParameters::GM: - // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) - return std::max(1.0, mu / params_.muStep); - case GncParameters::TLS: - // increases mu at each iteration (original cost is recovered for mu -> inf) - return mu * params_.muStep; - default: - throw std::runtime_error( - "GncOptimizer::updateMu: called with unknown loss type."); + case GncParameters::GM: + // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) + return std::max(1.0, mu / params_.muStep); + case GncParameters::TLS: + // increases mu at each iteration (original cost is recovered for mu -> inf) + return mu * params_.muStep; + 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 + /// 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: - 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."); + case GncParameters::GM: + 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_.verbosity >= GncParameters::Verbosity::SUMMARY) std::cout << "muConverged = true " << std::endl; return muConverged; } - /// check convergence of relative cost differences + /// Check convergence of relative cost differences. bool checkCostConvergence(const double cost, const double prev_cost) const { - bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost,1e-7) < params_.relativeCostTol; + bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) + < params_.relativeCostTol; if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) std::cout << "checkCostConvergence = true " << std::endl; return costConverged; } - /// check convergence of weights to binary values + /// Check convergence of weights to binary values. bool checkWeightsConvergence(const Vector& weights) const { - bool weightsConverged = false; - switch (params_.lossType) { + bool weightsConverged = false; + switch (params_.lossType) { case GncParameters::GM: - weightsConverged = false; // for GM, there is no clear binary convergence for the weights + 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 ){ + for (size_t i = 0; i < weights.size(); i++) { + if (std::fabs(weights[i] - std::round(weights[i])) + > params_.weightsTol) { weightsConverged = false; break; } @@ -245,23 +258,21 @@ class GncOptimizer { default: throw std::runtime_error( "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); - } - if (weightsConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) - std::cout << "weightsConverged = true " << std::endl; - return weightsConverged; } + if (weightsConverged + && params_.verbosity >= GncParameters::Verbosity::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 { - return checkCostConvergence(cost,prev_cost) || - checkWeightsConvergence(weights) || - checkMuConvergence(mu); + /// Check for convergence between consecutive GNC iterations. + bool checkConvergence(const double mu, const Vector& weights, + const double cost, const double prev_cost) const { + return checkCostConvergence(cost, prev_cost) + || checkWeightsConvergence(weights) || checkMuConvergence(mu); } - /// create a graph where each factor is weighted by the gnc weights + /// 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 NonlinearFactorGraph newGraph; @@ -287,7 +298,7 @@ class GncOptimizer { return newGraph; } - /// calculate gnc weights + /// Calculate gnc weights. Vector calculateWeights(const Values& currentEstimate, const double mu) { Vector weights = Vector::Ones(nfg_.size()); @@ -298,42 +309,43 @@ class GncOptimizer { } std::vector unknownWeights; std::set_difference(allWeights.begin(), allWeights.end(), - params_.knownInliers.begin(), params_.knownInliers.end(), - std::inserter(unknownWeights, unknownWeights.begin())); + params_.knownInliers.begin(), + params_.knownInliers.end(), + std::inserter(unknownWeights, unknownWeights.begin())); // update weights of known inlier/outlier measurements switch (params_.lossType) { - case GncParameters::GM: { // use eq (12) in GNC paper - for (size_t k : unknownWeights) { - 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); + case GncParameters::GM: { // use eq (12) in GNC paper + for (size_t k : unknownWeights) { + 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); + } } + return weights; } - return weights; - } - case GncParameters::TLS: { // use eq (14) in GNC paper - double upperbound = (mu + 1) / mu * params_.barcSq; - double lowerbound = mu / (mu + 1) * params_.barcSq; - for (size_t k : unknownWeights) { - if (nfg_[k]) { - double u2_k = nfg_[k]->error( - currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound) { - weights[k] = 0; - } else if (u2_k <= lowerbound) { - weights[k] = 1; - } else { - weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k) - mu; + case GncParameters::TLS: { // use eq (14) in GNC paper + double upperbound = (mu + 1) / mu * params_.barcSq; + double lowerbound = mu / (mu + 1) * params_.barcSq; + for (size_t k : unknownWeights) { + if (nfg_[k]) { + double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + if (u2_k >= upperbound) { + weights[k] = 0; + } else if (u2_k <= lowerbound) { + weights[k] = 1; + } else { + weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k) + - mu; + } } } + return weights; } - return weights; - } - default: - throw std::runtime_error( - "GncOptimizer::calculateWeights: called with unknown loss type."); + default: + throw std::runtime_error( + "GncOptimizer::calculateWeights: called with unknown loss type."); } } }; diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 30ceef4c72..9c4f21b81f 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -34,47 +34,50 @@ namespace gtsam { /* ************************************************************************* */ template class GncParams { -public: - /** For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer */ + public: + /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; - /** Verbosity levels */ + /// Verbosity levels enum Verbosity { - SILENT = 0, SUMMARY, VALUES + SILENT = 0, + SUMMARY, + VALUES }; - /** Choice of robust loss function for GNC */ + /// Choice of robust loss function for GNC. enum GncLossType { - GM /*Geman McClure*/, TLS /*Truncated least squares*/ + GM /*Geman McClure*/, + TLS /*Truncated least squares*/ }; - /// Constructor - GncParams(const BaseOptimizerParameters& baseOptimizerParams) : - baseOptimizerParams(baseOptimizerParams) { + /// Constructor. + GncParams(const BaseOptimizerParameters& baseOptimizerParams) + : baseOptimizerParams(baseOptimizerParams) { } - /// Default constructor - GncParams() : - baseOptimizerParams() { + /// Default constructor. + GncParams() + : baseOptimizerParams() { } - /// GNC parameters - BaseOptimizerParameters baseOptimizerParams; /*optimization parameters used to solve the weighted least squares problem at each GNC iteration*/ + /// GNC parameters. + BaseOptimizerParameters baseOptimizerParams; ///< Optimization parameters used to solve the weighted least squares problem at each GNC iteration /// 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) - Verbosity verbosity = SILENT; /* verbosity level */ - std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ + 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) + Verbosity verbosity = SILENT; ///< Verbosity level + std::vector knownInliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are inliers - /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType) + /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { lossType = type; } - /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended) + /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended). void setMaxIterations(const size_t maxIter) { std::cout << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " @@ -85,22 +88,24 @@ class GncParams { * 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 + * 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 + /// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep. void setMuStep(const double step) { muStep = step; } - /// Set the maximum relative difference in mu values to stop iterating - void setRelativeCostTol(double value) { relativeCostTol = value; + /// 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 maximum difference between the weights and their rounding in {0,1} to stop iterating. + void setWeightsTol(double value) { + weightsTol = value; } - /// Set the verbosity level + /// Set the verbosity level. void setVerbosityGNC(const Verbosity value) { verbosity = value; } @@ -108,33 +113,32 @@ class GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and - * only apply GNC to prune outliers from the loop closures + * only apply GNC to prune outliers from the loop closures. * */ void setKnownInliers(const std::vector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++) knownInliers.push_back(knownIn[i]); } - /// equals + /// 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 - && verbosity == other.verbosity - && knownInliers == other.knownInliers; + && verbosity == other.verbosity && knownInliers == other.knownInliers; } - /// print function + /// Print. void print(const std::string& str) const { std::cout << str << "\n"; switch (lossType) { - case GM: - std::cout << "lossType: Geman McClure" << "\n"; - break; - case TLS: - std::cout << "lossType: Truncated Least-squares" << "\n"; - break; - default: - throw std::runtime_error("GncParams::print: unknown loss type."); + case GM: + std::cout << "lossType: Geman McClure" << "\n"; + break; + case TLS: + std::cout << "lossType: Truncated Least-squares" << "\n"; + break; + default: + throw std::runtime_error("GncParams::print: unknown loss type."); } std::cout << "maxIterations: " << maxIterations << "\n"; std::cout << "barcSq: " << barcSq << "\n"; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 15a83eb4b9..f46563b919 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -73,16 +73,16 @@ TEST(GncOptimizer, gncParamsConstructor) { /* ************************************************************************* */ TEST(GncOptimizer, gncConstructor) { // has to have Gaussian noise models ! - auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor - // on a 2D point + auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor + // on a 2D point Point2 p0(3, 3); Values initial; initial.insert(X(1), p0); GncParams gncParams; - auto gnc = - GncOptimizer>(fg, initial, gncParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); CHECK(gnc.getFactors().equals(fg)); CHECK(gnc.getState().equals(initial)); @@ -100,8 +100,9 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { initial.insert(X(1), p0); GncParams gncParams; - auto gnc = GncOptimizer>( - fg_robust, initial, gncParams); + auto gnc = GncOptimizer>(fg_robust, + initial, + gncParams); // make sure that when parsing the graph is transformed into one without // robust loss @@ -118,19 +119,17 @@ TEST(GncOptimizer, initializeMu) { // testing GM mu initialization GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::GM); - auto gnc_gm = - GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType(GncParams::GncLossType::GM); + auto gnc_gm = GncOptimizer>(fg, initial, + gncParams); // 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); // testing TLS mu initialization - gncParams.setLossType( - GncParams::GncLossType::TLS); - auto gnc_tls = - GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType(GncParams::GncLossType::TLS); + auto gnc_tls = GncOptimizer>(fg, initial, + gncParams); // 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); @@ -146,11 +145,10 @@ TEST(GncOptimizer, updateMuGM) { initial.insert(X(1), p0); GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::GM); + gncParams.setLossType(GncParams::GncLossType::GM); gncParams.setMuStep(1.4); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); double mu = 5.0; EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol); @@ -171,10 +169,9 @@ TEST(GncOptimizer, updateMuTLS) { GncParams gncParams; gncParams.setMuStep(1.4); - gncParams.setLossType( - GncParams::GncLossType::TLS); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType(GncParams::GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); double mu = 5.0; EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu * 1.4, tol); @@ -190,24 +187,23 @@ TEST(GncOptimizer, checkMuConvergence) { initial.insert(X(1), p0); { - GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::GM); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + GncParams gncParams; + gncParams.setLossType(GncParams::GncLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); - double mu = 1.0; - CHECK(gnc.checkMuConvergence(mu)); + double mu = 1.0; + CHECK(gnc.checkMuConvergence(mu)); } { - GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::TLS); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + GncParams gncParams; + gncParams.setLossType( + GncParams::GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); - double mu = 1.0; - CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS + double mu = 1.0; + CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS } } @@ -221,26 +217,26 @@ TEST(GncOptimizer, checkCostConvergence) { initial.insert(X(1), p0); { - GncParams gncParams; - 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)); + GncParams gncParams; + 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)); } { - GncParams gncParams; - 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)); + GncParams gncParams; + 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)); } } @@ -254,48 +250,47 @@ TEST(GncOptimizer, checkWeightsConvergence) { initial.insert(X(1), p0); { - GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::GM); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + GncParams gncParams; + gncParams.setLossType(GncParams::GncLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); - Vector weights = Vector::Ones(fg.size()); - CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM + Vector weights = Vector::Ones(fg.size()); + CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM } { - GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::TLS); - auto gnc = - GncOptimizer>(fg, initial, gncParams); - - Vector weights = Vector::Ones(fg.size()); - // weights are binary, so checkWeightsConvergence = true - CHECK(gnc.checkWeightsConvergence(weights)); + GncParams gncParams; + gncParams.setLossType( + GncParams::GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Vector weights = Vector::Ones(fg.size()); + // weights are binary, so checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); } { - GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::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)); + GncParams gncParams; + gncParams.setLossType( + GncParams::GncLossType::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)); } { - GncParams gncParams; - gncParams.setLossType( - GncParams::GncLossType::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)); + GncParams gncParams; + gncParams.setLossType( + GncParams::GncLossType::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)); } } @@ -310,10 +305,9 @@ TEST(GncOptimizer, checkConvergenceTLS) { GncParams gncParams; gncParams.setRelativeCostTol(1e-5); - gncParams.setLossType( - GncParams::GncLossType::TLS); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + gncParams.setLossType(GncParams::GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); CHECK(gnc.checkCostConvergence(1.0, 1.0)); CHECK(!gnc.checkCostConvergence(1.0, 2.0)); @@ -333,12 +327,11 @@ TEST(GncOptimizer, calculateWeightsGM) { weights_expected[0] = 1.0; // zero error weights_expected[1] = 1.0; // zero error weights_expected[2] = 1.0; // zero error - weights_expected[3] = std::pow(1.0 / (50.0 + 1.0), 2); // outlier, error = 50 + weights_expected[3] = std::pow(1.0 / (50.0 + 1.0), 2); // outlier, error = 50 GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType( - GncParams::GncLossType::GM); + gncParams.setLossType(GncParams::GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -346,11 +339,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 + weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 gncParams.setInlierCostThreshold(barcSq); - auto gnc2 = - GncOptimizer>(fg, initial, gncParams); + auto gnc2 = GncOptimizer>(fg, initial, + gncParams); weights_actual = gnc2.calculateWeights(initial, mu); CHECK(assert_equal(weights_expected, weights_actual, tol)); } @@ -372,8 +364,7 @@ TEST(GncOptimizer, calculateWeightsTLS) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType( - GncParams::GncLossType::TLS); + gncParams.setLossType(GncParams::GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -391,45 +382,44 @@ TEST(GncOptimizer, calculateWeightsTLS2) { // create very simple factor graph with a single factor 0.5 * 1/sigma^2 * || x - [1;0] ||^2 double sigma = 1; - SharedDiagonal noise = - noiseModel::Diagonal::Sigmas(Vector2(sigma, sigma)); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector2(sigma, sigma)); NonlinearFactorGraph nfg; - nfg.add(PriorFactor(X(1),x_prior,noise)); + nfg.add(PriorFactor(X(1), x_prior, noise)); // cost of the factor: - DOUBLES_EQUAL(0.5 * 1/(sigma*sigma), nfg.error(initial), tol); + DOUBLES_EQUAL(0.5 * 1 / (sigma * sigma), nfg.error(initial), tol); // check the TLS weights are correct: CASE 1: residual below barcsq { - // expected: - Vector weights_expected = Vector::Zero(1); - weights_expected[0] = 1.0; // inlier - // actual: - GaussNewtonParams gnParams; - GncParams gncParams(gnParams); - gncParams.setLossType( - GncParams::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); - double mu = 1e6; - Vector weights_actual = gnc.calculateWeights(initial, mu); - CHECK(assert_equal(weights_expected, weights_actual, tol)); + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 1.0; // inlier + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncParams::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); + double mu = 1e6; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); } // check the TLS weights are correct: CASE 2: residual above barcsq { - // expected: - Vector weights_expected = Vector::Zero(1); - weights_expected[0] = 0.0; // outlier - // actual: - GaussNewtonParams gnParams; - GncParams gncParams(gnParams); - gncParams.setLossType( - GncParams::GncLossType::TLS); - 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); - CHECK(assert_equal(weights_expected, weights_actual, tol)); + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 0.0; // outlier + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncParams::GncLossType::TLS); + 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); + CHECK(assert_equal(weights_expected, weights_actual, tol)); } // check the TLS weights are correct: CASE 2: residual at barcsq { @@ -439,11 +429,11 @@ TEST(GncOptimizer, calculateWeightsTLS2) { // actual: GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType( - GncParams::GncLossType::TLS); - 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 + gncParams.setLossType(GncParams::GncLossType::TLS); + 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); CHECK(assert_equal(weights_expected, weights_actual, 1e-5)); } @@ -453,17 +443,16 @@ TEST(GncOptimizer, calculateWeightsTLS2) { TEST(GncOptimizer, makeWeightedGraph) { // create original factor double sigma1 = 0.1; - NonlinearFactorGraph nfg = - example::nonlinearFactorGraphWithGivenSigma(sigma1); + NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma( + sigma1); // create expected double sigma2 = 10; - NonlinearFactorGraph expected = - example::nonlinearFactorGraphWithGivenSigma(sigma2); + NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma( + sigma2); // create weights - Vector weights = Vector::Ones( - 1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 + Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 weights[0] = 1e-4; // create actual @@ -491,8 +480,8 @@ TEST(GncOptimizer, optimizeSimple) { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); - auto gnc = - GncOptimizer>(fg, initial, gncParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); Values actual = gnc.optimize(); DOUBLES_EQUAL(0, fg.error(actual), tol); @@ -515,15 +504,13 @@ TEST(GncOptimizer, optimize) { CHECK(assert_equal(Point2(0.25, 0.0), gn_results.at(X(1)), 1e-3)); // try with robust loss function and standard GN - auto fg_robust = - example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with - // factors wrapped in - // Geman McClure losses + auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with + // factors wrapped in + // Geman McClure losses GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); Values gn2_results = gn2.optimize(); // converges to incorrect point, this time due to the nonconvexity of the loss - CHECK( - assert_equal(Point2(0.999706, 0.0), gn2_results.at(X(1)), 1e-3)); + CHECK(assert_equal(Point2(0.999706, 0.0), gn2_results.at(X(1)), 1e-3)); // .. but graduated nonconvexity ensures both robustness and convergence in // the face of nonconvexity @@ -549,59 +536,59 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { // nonconvexity with known inliers { - GncParams gncParams; - gncParams.setKnownInliers(knownInliers); - gncParams.setLossType( - GncParams::GncLossType::GM); - //gncParams.setVerbosityGNC(GncParams::Verbosity::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); + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncParams::GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::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); } { - GncParams gncParams; - gncParams.setKnownInliers(knownInliers); - gncParams.setLossType( - GncParams::GncLossType::TLS); - // gncParams.setVerbosityGNC(GncParams::Verbosity::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); + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncParams::GncLossType::TLS); + // gncParams.setVerbosityGNC(GncParams::Verbosity::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::GncLossType::TLS); - //gncParams.setVerbosityGNC(GncParams::Verbosity::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); + // if we set the threshold large, they are all inliers + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncParams::GncLossType::TLS); + //gncParams.setVerbosityGNC(GncParams::Verbosity::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); } } @@ -613,24 +600,22 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { Values::shared_ptr initial; boost::tie(graph, initial) = load2D(filename); // Add a Gaussian prior on first poses - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = - noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.01, 0.01, 0.01)); graph->addPrior(0, priorMean, priorNoise); /// get expected values by optimizing outlier-free graph Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); // add a few outliers - SharedDiagonal betweenNoise = - noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01)); - graph->push_back(BetweenFactor( - 90, 50, Pose2(), - betweenNoise)); // some arbitrary and incorrect between factor + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.1, 0.1, 0.01)); + graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); // some arbitrary and incorrect between factor /// get expected values by optimizing outlier-free graph - Values expectedWithOutliers = - LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial) + .optimize(); // as expected, the following test fails due to the presence of an outlier! // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); @@ -639,13 +624,12 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // inliers, but this problem is simple enought to succeed even without that // assumption std::vector knownInliers; GncParams gncParams; - auto gnc = - GncOptimizer>(*graph, *initial, gncParams); + auto gnc = GncOptimizer>(*graph, *initial, + gncParams); Values actual = gnc.optimize(); // compare - CHECK( - assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! + CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! } /* ************************************************************************* */ From 24672385b377962ae9bb87ec04cf7726bc43c8b6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 29 Dec 2020 21:59:21 -0500 Subject: [PATCH 44/45] moved gncLossType outside params --- gtsam/nonlinear/GncOptimizer.h | 20 ++++++++--------- gtsam/nonlinear/GncParams.h | 12 +++++----- tests/testGncOptimizer.cpp | 40 +++++++++++++++++----------------- 3 files changed, 36 insertions(+), 36 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index bbc3b9f849..cfabf0ab62 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -176,10 +176,10 @@ class GncOptimizer { } // set initial mu switch (params_.lossType) { - case GncParameters::GM: + case GncLossType::GM: // surrogate cost is convex for large mu return 2 * rmax_sq / params_.barcSq; // initial mu - case GncParameters::TLS: + case GncLossType::TLS: /* 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) @@ -198,10 +198,10 @@ class GncOptimizer { /// Update the gnc parameter mu to gradually increase nonconvexity. double updateMu(const double mu) const { switch (params_.lossType) { - case GncParameters::GM: + case GncLossType::GM: // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) return std::max(1.0, mu / params_.muStep); - case GncParameters::TLS: + case GncLossType::TLS: // increases mu at each iteration (original cost is recovered for mu -> inf) return mu * params_.muStep; default: @@ -214,10 +214,10 @@ class GncOptimizer { bool checkMuConvergence(const double mu) const { bool muConverged = false; switch (params_.lossType) { - case GncParameters::GM: + case GncLossType::GM: muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function break; - case GncParameters::TLS: + case GncLossType::TLS: muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity) break; default: @@ -242,10 +242,10 @@ class GncOptimizer { bool checkWeightsConvergence(const Vector& weights) const { bool weightsConverged = false; switch (params_.lossType) { - case GncParameters::GM: + case GncLossType::GM: weightsConverged = false; // for GM, there is no clear binary convergence for the weights break; - case GncParameters::TLS: + case GncLossType::TLS: weightsConverged = true; for (size_t i = 0; i < weights.size(); i++) { if (std::fabs(weights[i] - std::round(weights[i])) @@ -315,7 +315,7 @@ class GncOptimizer { // update weights of known inlier/outlier measurements switch (params_.lossType) { - case GncParameters::GM: { // use eq (12) in GNC paper + case GncLossType::GM: { // use eq (12) in GNC paper for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual @@ -325,7 +325,7 @@ class GncOptimizer { } return weights; } - case GncParameters::TLS: { // use eq (14) in GNC paper + case GncLossType::TLS: { // use eq (14) in GNC paper double upperbound = (mu + 1) / mu * params_.barcSq; double lowerbound = mu / (mu + 1) * params_.barcSq; for (size_t k : unknownWeights) { diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 9c4f21b81f..5f130ddf20 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -32,6 +32,12 @@ namespace gtsam { /* ************************************************************************* */ +/// Choice of robust loss function for GNC. +enum GncLossType { + GM /*Geman McClure*/, + TLS /*Truncated least squares*/ +}; + template class GncParams { public: @@ -45,12 +51,6 @@ class GncParams { VALUES }; - /// Choice of robust loss function for GNC. - enum GncLossType { - GM /*Geman McClure*/, - TLS /*Truncated least squares*/ - }; - /// Constructor. GncParams(const BaseOptimizerParameters& baseOptimizerParams) : baseOptimizerParams(baseOptimizerParams) { diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index f46563b919..738c77936a 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::GncLossType::GM); + gncParams2c.setLossType(GncLossType::GM); CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); } @@ -119,7 +119,7 @@ TEST(GncOptimizer, initializeMu) { // testing GM mu initialization GncParams gncParams; - gncParams.setLossType(GncParams::GncLossType::GM); + gncParams.setLossType(GncLossType::GM); auto gnc_gm = GncOptimizer>(fg, initial, gncParams); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq @@ -127,7 +127,7 @@ TEST(GncOptimizer, initializeMu) { EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3); // testing TLS mu initialization - gncParams.setLossType(GncParams::GncLossType::TLS); + gncParams.setLossType(GncLossType::TLS); auto gnc_tls = GncOptimizer>(fg, initial, gncParams); // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) @@ -145,7 +145,7 @@ TEST(GncOptimizer, updateMuGM) { initial.insert(X(1), p0); GncParams gncParams; - gncParams.setLossType(GncParams::GncLossType::GM); + gncParams.setLossType(GncLossType::GM); gncParams.setMuStep(1.4); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -169,7 +169,7 @@ TEST(GncOptimizer, updateMuTLS) { GncParams gncParams; gncParams.setMuStep(1.4); - gncParams.setLossType(GncParams::GncLossType::TLS); + gncParams.setLossType(GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -188,7 +188,7 @@ TEST(GncOptimizer, checkMuConvergence) { { GncParams gncParams; - gncParams.setLossType(GncParams::GncLossType::GM); + gncParams.setLossType(GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -198,7 +198,7 @@ TEST(GncOptimizer, checkMuConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::GncLossType::TLS); + GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -251,7 +251,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; - gncParams.setLossType(GncParams::GncLossType::GM); + gncParams.setLossType(GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -261,7 +261,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::GncLossType::TLS); + GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -272,7 +272,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::GncLossType::TLS); + GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -283,7 +283,7 @@ TEST(GncOptimizer, checkWeightsConvergence) { { GncParams gncParams; gncParams.setLossType( - GncParams::GncLossType::TLS); + GncLossType::TLS); gncParams.setWeightsTol(0.1); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -305,7 +305,7 @@ TEST(GncOptimizer, checkConvergenceTLS) { GncParams gncParams; gncParams.setRelativeCostTol(1e-5); - gncParams.setLossType(GncParams::GncLossType::TLS); + gncParams.setLossType(GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -331,7 +331,7 @@ TEST(GncOptimizer, calculateWeightsGM) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType(GncParams::GncLossType::GM); + gncParams.setLossType(GncLossType::GM); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -364,7 +364,7 @@ TEST(GncOptimizer, calculateWeightsTLS) { GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType(GncParams::GncLossType::TLS); + gncParams.setLossType(GncLossType::TLS); auto gnc = GncOptimizer>(fg, initial, gncParams); double mu = 1.0; Vector weights_actual = gnc.calculateWeights(initial, mu); @@ -397,7 +397,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { // actual: GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType(GncParams::GncLossType::TLS); + 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); @@ -413,7 +413,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { // actual: GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType(GncParams::GncLossType::TLS); + 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); @@ -429,7 +429,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) { // actual: GaussNewtonParams gnParams; GncParams gncParams(gnParams); - gncParams.setLossType(GncParams::GncLossType::TLS); + 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); @@ -538,7 +538,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { { GncParams gncParams; gncParams.setKnownInliers(knownInliers); - gncParams.setLossType(GncParams::GncLossType::GM); + gncParams.setLossType(GncLossType::GM); //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -555,7 +555,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { { GncParams gncParams; gncParams.setKnownInliers(knownInliers); - gncParams.setLossType(GncParams::GncLossType::TLS); + gncParams.setLossType(GncLossType::TLS); // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); @@ -574,7 +574,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { // if we set the threshold large, they are all inliers GncParams gncParams; gncParams.setKnownInliers(knownInliers); - gncParams.setLossType(GncParams::GncLossType::TLS); + gncParams.setLossType(GncLossType::TLS); //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); gncParams.setInlierCostThreshold(100.0); auto gnc = GncOptimizer>(fg, initial, From 248eec8e41dc191db511fd7d6c7a5d7367b4f084 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 30 Dec 2020 14:13:40 -0500 Subject: [PATCH 45/45] addressed final comments by Frank --- gtsam/nonlinear/GncOptimizer.h | 35 ++++++++++------------------------ gtsam/nonlinear/GncParams.h | 9 +++++++++ 2 files changed, 19 insertions(+), 25 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index cfabf0ab62..cd0b4c81a4 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -57,39 +57,25 @@ class GncOptimizer { if (graph[i]) { NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< NoiseModelFactor>(graph[i]); - noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast< + auto robust = boost::dynamic_pointer_cast< noiseModel::Robust>(factor->noiseModel()); - if (robust) { // if the factor has a robust loss, we have to change it: - SharedNoiseModel gaussianNoise = robust->noise(); - NoiseModelFactor::shared_ptr gaussianFactor = factor - ->cloneWithNewNoiseModel(gaussianNoise); - nfg_[i] = gaussianFactor; - } else { // else we directly push it back - nfg_[i] = factor; - } + // if the factor has a robust loss, we remove the robust loss + nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; } } } /// Access a copy of the internal factor graph. - NonlinearFactorGraph getFactors() const { - return NonlinearFactorGraph(nfg_); - } + const NonlinearFactorGraph& getFactors() const { return nfg_; } /// Access a copy of the internal values. - Values getState() const { - return Values(state_); - } + const Values& getState() const { return state_; } /// Access a copy of the parameters. - GncParameters getParams() const { - return GncParameters(params_); - } + const GncParameters& getParams() const { return params_;} /// Access a copy of the GNC weights. - Vector getWeights() const { - return weights_; - } + const Vector& getWeights() const { return weights_;} /// Compute optimal solution using graduated non-convexity. Values optimize() { @@ -279,15 +265,14 @@ class GncOptimizer { newGraph.resize(nfg_.size()); for (size_t i = 0; i < nfg_.size(); i++) { if (nfg_[i]) { - NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + auto factor = boost::dynamic_pointer_cast< NoiseModelFactor>(nfg_[i]); - noiseModel::Gaussian::shared_ptr noiseModel = + auto noiseModel = boost::dynamic_pointer_cast( factor->noiseModel()); if (noiseModel) { Matrix newInfo = weights[i] * noiseModel->information(); - SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information( - newInfo); + auto newNoiseModel = noiseModel::Gaussian::Information(newInfo); newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); } else { throw std::runtime_error( diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 5f130ddf20..0388a7fd16 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -77,6 +77,7 @@ class GncParams { void setLossType(const GncLossType type) { lossType = type; } + /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended). void setMaxIterations(const size_t maxIter) { std::cout @@ -84,6 +85,7 @@ class GncParams { << std::endl; 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. @@ -93,22 +95,27 @@ class GncParams { 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; } + /// 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 Verbosity value) { verbosity = value; } + /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. @@ -119,6 +126,7 @@ class GncParams { for (size_t i = 0; i < knownIn.size(); i++) knownInliers.push_back(knownIn[i]); } + /// Equals. bool equals(const GncParams& other, double tol = 1e-9) const { return baseOptimizerParams.equals(other.baseOptimizerParams) @@ -127,6 +135,7 @@ class GncParams { && std::fabs(muStep - other.muStep) <= tol && verbosity == other.verbosity && knownInliers == other.knownInliers; } + /// Print. void print(const std::string& str) const { std::cout << str << "\n";