From 7e29944f95c6afb4f9043b8c912add0f558b8852 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Wed, 25 Nov 2020 11:02:01 -0500 Subject: [PATCH 01/61] 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 000000000..d9ba209c5 --- /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/61] starting to create test and code for gncParams --- tests/testGncOptimizer.cpp | 105 ++++++++++++++++++++----------------- 1 file changed, 57 insertions(+), 48 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index d9ba209c5..878808505 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 +//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 +// +// } +//}; - 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, 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/61] 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 a7bc10a1f..218230421 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -113,6 +113,17 @@ public: 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 6415174d5..295721cc4 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/61] 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 878808505..1d3057335 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include @@ -37,44 +38,51 @@ public: // 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 @@ public: //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/61] added gnc loop --- tests/testGncOptimizer.cpp | 206 +++++++++++++++++++++++++++++++------ 1 file changed, 174 insertions(+), 32 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 1d3057335..87ac26841 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -29,21 +29,70 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::L; +static double tol = 1e-7; /* ************************************************************************* */ template class GncParams { public: - // using BaseOptimizer = BaseOptimizerParameters::OptimizerType; - GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {} + /** See NonlinearOptimizerParams::verbosity */ + enum RobustLossType { + GM /*Geman McClure*/, TLS /*Truncated least squares*/ + }; + + // using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType; + + GncParams(const BaseOptimizerParameters& baseOptimizerParams): + baseOptimizerParams(baseOptimizerParams), + lossType(GM), /* default loss*/ + maxIterations(100), /* maximum number of iterations*/ + barcSq(1.0), /* a factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance*/ + muStep(1.4){}/* multiplicative factor to reduce/increase the mu in gnc */ // default constructor GncParams(): baseOptimizerParams() {} BaseOptimizerParameters baseOptimizerParams; - /// any other specific GNC parameters: + RobustLossType lossType; + size_t maxIterations; + double barcSq; + double muStep; + + void setLossType(RobustLossType type){ lossType = type; } + void setMaxIterations(size_t maxIter){ + std::cout + << "setMaxIterations: changing the max number of iterations might lead to less accurate solutions and is not recommended! " + << std::endl; + maxIterations = maxIter; + } + void setInlierThreshold(double inth){ barcSq = inth; } + void setMuStep(double step){ muStep = step; } + + /// equals + bool equals(const GncParams& other, double tol = 1e-9) const { + return baseOptimizerParams.equals(other.baseOptimizerParams) + && lossType == other.lossType + && maxIterations == other.maxIterations + && std::fabs(barcSq - other.barcSq) <= tol + && std::fabs(muStep - other.muStep) <= tol; + } + + /// print function + void print(const std::string& str) const { + std::cout << str << "\n"; + switch(lossType) { + case GM: std::cout << "lossType: Geman McClure" << "\n"; break; + default: + throw std::runtime_error( + "GncParams::print: unknown loss type."); + } + std::cout << "maxIterations: " << maxIterations << "\n"; + std::cout << "barcSq: " << barcSq << "\n"; + std::cout << "muStep: " << muStep << "\n"; + baseOptimizerParams.print(str); + } }; /* ************************************************************************* */ @@ -64,33 +113,88 @@ public: // TODO: Check that all noise models are Gaussian } -// Values optimize() const { -// NonlinearFactorGraph currentGraph = graph_; -// for (i : {1, 2, 3}) { -// BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial); -// VALUES currentSolution = baseOptimizer.optimize(); -// if (converged) { -// return currentSolution; -// } -// graph_i = this->makeGraph(currentSolution); -// } -//} + NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); } + Values getState() const { return Values(state_); } + GncParameters getParams() const { return GncParameters(params_); } -//NonlinearFactorGraph makeGraph(const Values& currentSolution) const { -// // calculate some weights -// this->calculateWeights(); -// // copy the graph with new weights -// -//} + /// implement GNC main loop, including graduating nonconvexity with mu + Values optimize() { + // start by assuming all measurements are inliers + Vector weights = Vector::Ones(nfg_.size()); + GaussNewtonOptimizer baseOptimizer(nfg_,state_); + Values result = baseOptimizer.optimize(); + double mu = initializeMu(); + for(size_t iter=0; iter < params_.maxIterations; iter++){ + // weights update + weights = calculateWeights(result, mu); + + // variable/values update + NonlinearFactorGraph graph_iter = this->makeGraph(weights); + GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); + Values result = baseOptimizer.optimize(); + + // stopping condition + if( checkMuConvergence(mu) ) { break; } + + // otherwise update mu + mu = updateMu(mu); + } + return result; + } + + /// initialize the gnc parameter mu such that loss is approximately convex + double initializeMu() const { + // compute largest error across all factors + double rmax_sq = 0.0; + for (size_t i = 0; i < nfg_.size(); i++) { + if(nfg_[i]){ + rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_)); + } + } + // set initial mu + switch(params_.lossType) { + case GncParameters::GM: + return 2*rmax_sq / params_.barcSq; // initial mu + default: + throw std::runtime_error( + "GncOptimizer::initializeMu: called with unknown loss type."); + } + } + + /// update the gnc parameter mu to gradually increase nonconvexity + double updateMu(const double mu) const { + switch(params_.lossType) { + case GncParameters::GM: + return std::max(1.0 , mu / params_.muStep); // reduce mu, but saturate at 1 + default: + throw std::runtime_error( + "GncOptimizer::updateMu: called with unknown loss type."); + } + } + + /// check if we have reached the value of mu for which the surrogate loss matches the original loss + bool checkMuConvergence(const double mu) const { + switch(params_.lossType) { + case GncParameters::GM: + return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + default: + throw std::runtime_error( + "GncOptimizer::checkMuConvergence: called with unknown loss type."); + } + } + + /// create a graph where each factor is weighted by the gnc weights + NonlinearFactorGraph makeGraph(const Vector& weights) const { + return NonlinearFactorGraph(nfg_); + } + + /// calculate gnc weights + Vector calculateWeights(const Values currentEstimate, const double mu){ + Vector weights = Vector::Ones(nfg_.size()); + return weights; + } }; -///* ************************************************************************* */ -//TEST(GncOptimizer, calculateWeights) { -//} -// -///* ************************************************************************* */ -//TEST(GncOptimizer, copyGraph) { -//} /* ************************************************************************* */ TEST(GncOptimizer, gncParamsConstructor) { @@ -106,7 +210,7 @@ TEST(GncOptimizer, gncParamsConstructor) { // and check params become different if we change lmParams lmParams.setVerbosity("DELTA"); - CHECK(!lmParams.equals(gncParams1.baseOptimizerParams)); + CHECK(! lmParams.equals(gncParams1.baseOptimizerParams)); // and same for GN GaussNewtonParams gnParams; @@ -116,9 +220,44 @@ TEST(GncOptimizer, gncParamsConstructor) { // check default constructor GncParams gncParams2b; CHECK(gnParams.equals(gncParams2b.baseOptimizerParams)); + + // change something at the gncParams level + GncParams gncParams2c(gncParams2b); + gncParams2c.setLossType(GncParams::RobustLossType::TLS); + CHECK(! gncParams2c.equals(gncParams2b.baseOptimizerParams)); } /* ************************************************************************* */ +TEST(GncOptimizer, gncConstructor) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + CHECK(gnc.getFactors().equals(fg)); + CHECK(gnc.getState().equals(initial)); + CHECK(gnc.getParams().equals(gncParams)); +} + +///* ************************************************************************* */ +//TEST(GncOptimizer, calculateWeights) { +//} +// +///* ************************************************************************* */ +//TEST(GncOptimizer, calculateWeights) { +//} +// +///* ************************************************************************* */ +//TEST(GncOptimizer, copyGraph) { +//} + +/* ************************************************************************* * TEST(GncOptimizer, makeGraph) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point @@ -134,7 +273,7 @@ TEST(GncOptimizer, makeGraph) { // NonlinearFactorGraph actual = gnc.makeGraph(initial); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST(GncOptimizer, optimize) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -144,10 +283,13 @@ TEST(GncOptimizer, optimize) { initial.insert(X(1), p0); LevenbergMarquardtParams lmParams; - GncParams gncParams(lmParams); - auto gnc = GncOptimizer(fg, initial, gncParams); + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + gncParams.print(""); + Values actual = gnc.optimize(); - DOUBLES_EQUAL(0, fg.error(actual2), tol); + DOUBLES_EQUAL(0, fg.error(actual), tol); } /* ************************************************************************* */ From a33c50fcefb42dd85276624b83366d1aedfc0c46 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 15:46:12 -0500 Subject: [PATCH 06/61] 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 0c933d106..271fb0581 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 87ac26841..1951e51f1 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/61] 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 1951e51f1..bfea5977a 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 @@ public: /// 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/61] 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 271fb0581..70cda1eb0 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 bfea5977a..d770f58a8 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/61] 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 d770f58a8..a31f4b677 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -195,7 +195,7 @@ public: 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/61] 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 a31f4b677..a2dcafc81 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -113,7 +113,15 @@ 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 + + // 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 @@ public: 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/61] 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 1cfcba274..8b8d2da6c 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 80fbfbb11..00311fb87 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -244,6 +244,12 @@ public: */ 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 70cda1eb0..944899e70 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 662b071df..84bba850b 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/61] 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 a2dcafc81..e80ad7974 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -112,14 +112,18 @@ private: 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/61] 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 e80ad7974..502eff520 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -115,14 +115,17 @@ public: 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/61] 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 502eff520..73584882f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -147,7 +147,7 @@ public: 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 @@ public: } /// 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/61] added verbosity --- tests/testGncOptimizer.cpp | 49 ++++++++++++++++++++++++++++---------- 1 file changed, 36 insertions(+), 13 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 73584882f..630b0c92b 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 @@ public: 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 @@ public: 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 @@ public: && 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 @@ public: 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 @@ public: 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 @@ public: 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/61] 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 630b0c92b..383544c6e 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -48,7 +48,7 @@ public: 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 @@ public: // 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/61] 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 383544c6e..2e7692bec 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -51,34 +51,34 @@ public: 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 @@ public: 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 @@ public: /// 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/61] correct formatting --- tests/testGncOptimizer.cpp | 275 +++++++++++++++++++++---------------- 1 file changed, 155 insertions(+), 120 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 2e7692bec..65abc2042 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 @@ public: 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 @@ public: 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 @@ public: } /// 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 @@ public: 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 @@ public: // 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 @@ public: /// 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 @@ public: /// 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 @@ public: 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 @@ public: } /// 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 @@ public: /* ************************************************************************* */ 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(); + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); - Point2 p0(0, 0); - Values initial; - initial.insert(X(1), p0); + 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 + // 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)); + 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)); + 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/61] 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 65abc2042..8415ec3cc 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/61] 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 000000000..6601494e8 --- /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 8415ec3cc..5006aa941 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/61] 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 6601494e8..7c41b2475 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -48,16 +48,18 @@ public: 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 @@ public: 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 @@ public: && verbosityGNC == other.verbosityGNC && knownInliers == other.knownInliers; } - /// print function void print(const std::string& str) const { std::cout << str << "\n"; @@ -132,6 +145,7 @@ private: 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 @@ public: } } - /// 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 @@ public: } 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/61] 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 7c41b2475..d540dffbb 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/61] 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 d540dffbb..be7d046c6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -119,6 +119,9 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: } } 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/61] 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 be7d046c6..b6e6933ec 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -65,6 +65,7 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: break; } // otherwise update mu + mu_prev = mu; mu = updateMu(mu); } return result; @@ -279,11 +286,12 @@ public: } /// 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 @@ public: } } 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 5006aa941..3f784b96e 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/61] 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 b6e6933ec..40ed8c49a 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/61] 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 40ed8c49a..f5412c6ce 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -340,7 +340,7 @@ public: // 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 @@ public: } } 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/61] mu initialization test & minor formatting fixes --- tests/testGncOptimizer.cpp | 135 +++++++++++++++++++++++-------------- 1 file changed, 84 insertions(+), 51 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 3f784b96e..ca40231c9 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 +#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/61] 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 ca40231c9..a1c6fe526 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/61] 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 f5412c6ce..811487779 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -203,9 +203,11 @@ public: 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 @@ public: 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 @@ public: } 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/61] test fix --- tests/testGncOptimizer.cpp | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a1c6fe526..5734dfc43 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)); +} - // TODO: test relative mu convergence +/* ************************************************************************* */ +TEST(GncOptimizer, checkMuConvergenceTLS) { + // 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::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/61] 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 5734dfc43..b3bef11e0 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/61] 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 a436597e5..3ef4dfdeb 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 811487779..730c1aa43 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 @@ public: 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 @@ public: 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 @@ public: // 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 4962ad366..20a8eb4c1 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 @@ public: 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/61] 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 730c1aa43..dd6c8d17d 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 @@ public: 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/61] 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 dd6c8d17d..d4b25409f 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -264,11 +264,12 @@ public: // 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 @@ public: 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 b3bef11e0..85a196bd7 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/61] 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 d4b25409f..df87c4433 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -83,8 +83,11 @@ public: } /** 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 85a196bd7..7c4fbd5fa 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/61] Fix TLS convergence check --- gtsam/nonlinear/GncOptimizer.h | 61 +++++++++++++++++++++++++++------- tests/testGncOptimizer.cpp | 4 +-- 2 files changed, 51 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index df87c4433..0f4d5ea27 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -66,7 +66,7 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: // weights update weights_ = calculateWeights(result, mu); + // update cost + prev_cost = cost; + cost = calculateWeightedCost(); + // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); BaseOptimizer baseOptimizer_iter(graph_iter, state_); result = baseOptimizer_iter.optimize(); - // 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 @@ public: } break; } + + // update mu + mu = updateMu(mu); } return result; } @@ -295,19 +298,53 @@ public: } } + /// calculated sum of weighted squared residuals + double calculateWeightedCost() const { + double cost = 0; + for (size_t i = 0; i < nfg_.size(); i++) { + cost += weights_[i] * nfg_[i]->error(state_); + } + return cost; + } + /// check if we have reached the value of mu for which the surrogate loss matches the original loss - bool checkMuConvergence(const double mu, const 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 - case GncParameters::TLS: - return std::fabs(mu - mu_prev) < params_.relativeMuTol; 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 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 7c4fbd5fa..a0e371463 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/61] 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 0f4d5ea27..d47b6bc82 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -95,7 +95,7 @@ public: 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/61] fix test --- tests/testGncOptimizer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a0e371463..dc96762b8 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/61] - fixed stopping conditions - handled degenerate case in mu initialization - set TLS as default - added more unit tests --- gtsam/nonlinear/GncOptimizer.h | 128 +++++++++++++++++---------- tests/testGncOptimizer.cpp | 155 ++++++++++++++++++++++++++++++++- 2 files changed, 233 insertions(+), 50 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d47b6bc82..b66e0f523 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -62,11 +62,12 @@ public: /// GNC parameters BaseOptimizerParameters baseOptimizerParams; /*optimization parameters used to solve the weighted least squares problem at each GNC iteration*/ /// any other specific GNC parameters: - RobustLossType lossType = GM; /* default loss*/ + RobustLossType lossType = TLS; /* default loss*/ size_t maxIterations = 100; /* maximum number of iterations*/ double barcSq = 1.0; /* a factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance*/ double muStep = 1.4; /* multiplicative factor to reduce/increase the mu in gnc */ - double relativeCostTol = 1e-5; ///< The maximum relative cost change to stop iterating + double relativeCostTol = 1e-5; ///< if relative cost change if below this threshold, stop iterating + double weightsTol = 1e-4; ///< if the weights are within weightsTol from being binary, stop iterating (only for TLS) VerbosityGNC verbosityGNC = SILENT; /* verbosity level */ std::vector knownInliers = std::vector(); /* slots in the factor graph corresponding to measurements that we know are inliers */ @@ -97,6 +98,9 @@ public: /// Set the maximum relative difference in mu values to stop iterating void setRelativeCostTol(double value) { relativeCostTol = value; } + /// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating + void setWeightsTol(double value) { weightsTol = value; + } /// Set the verbosity level void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; @@ -136,6 +140,8 @@ public: std::cout << "maxIterations: " << maxIterations << "\n"; std::cout << "barcSq: " << barcSq << "\n"; std::cout << "muStep: " << muStep << "\n"; + std::cout << "relativeCostTol: " << relativeCostTol << "\n"; + std::cout << "weightsTol: " << weightsTol << "\n"; std::cout << "verbosityGNC: " << verbosityGNC << "\n"; for (size_t i = 0; i < knownInliers.size(); i++) std::cout << "knownInliers: " << knownInliers[i] << "\n"; @@ -205,8 +211,8 @@ public: BaseOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); - double cost = calculateWeightedCost(); - double prev_cost = cost; + double prev_cost = nfg_.error(result); + double cost = 0.0; // this will be updated in the main loop // handle the degenerate case that corresponds to small // maximum residual errors at initialization @@ -215,16 +221,21 @@ public: if (mu <= 0) { if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " - "initialization is small." << std::endl; + "initialization is small." << std::endl; + } + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { result.print("result\n"); + std::cout << "mu: " << mu << std::endl; } return result; } - for (size_t iter = 0; iter < params_.maxIterations; iter++) { + size_t iter; + for (iter = 0; iter < params_.maxIterations; iter++) { // display info if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + std::cout << "iter: " << iter << std::endl; result.print("result\n"); std::cout << "mu: " << mu << std::endl; std::cout << "weights: " << weights_ << std::endl; @@ -232,28 +243,34 @@ public: // weights update weights_ = calculateWeights(result, mu); - // update cost - prev_cost = cost; - cost = calculateWeightedCost(); - // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); BaseOptimizer baseOptimizer_iter(graph_iter, state_); result = baseOptimizer_iter.optimize(); // stopping condition - if (checkConvergence(mu, cost, prev_cost)) { - // display info - if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { - std::cout << "final iterations: " << iter << std::endl; - std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; - } - break; - } + cost = graph_iter.error(result); + if (checkConvergence(mu, weights_, cost, prev_cost)) { break; } // update mu mu = updateMu(mu); + + // get ready for next iteration + prev_cost = cost; + + // display info + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) { + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; + } + } + // display info + if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) { + std::cout << "final iterations: " << iter << std::endl; + std::cout << "final mu: " << mu << std::endl; + std::cout << "final weights: " << weights_ << std::endl; + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; } return result; } @@ -276,7 +293,9 @@ public: // initialize mu to the value specified in Remark 5 in GNC paper. // surrogate cost is convex for mu close to zero // degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop) - return params_.barcSq / (2 * rmax_sq - params_.barcSq); + // according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual + // however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop + return (2 * rmax_sq - params_.barcSq) > 0 ? params_.barcSq / (2 * rmax_sq - params_.barcSq) : -1; default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type."); @@ -298,53 +317,68 @@ public: } } - /// calculated sum of weighted squared residuals - double calculateWeightedCost() const { - double cost = 0; - for (size_t i = 0; i < nfg_.size(); i++) { - cost += weights_[i] * nfg_[i]->error(state_); - } - return cost; - } - /// check if we have reached the value of mu for which the surrogate loss matches the original loss bool checkMuConvergence(const double mu) const { + bool muConverged = false; switch (params_.lossType) { case GncParameters::GM: - return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + break; + case GncParameters::TLS: + muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity) + break; default: throw std::runtime_error( "GncOptimizer::checkMuConvergence: called with unknown loss type."); } + if (muConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + std::cout << "muConverged = true " << std::endl; + return muConverged; } /// check convergence of relative cost differences bool checkCostConvergence(const double cost, const double prev_cost) const { - switch (params_.lossType) { - case GncParameters::TLS: - return std::fabs(cost - prev_cost) < params_.relativeCostTol; - default: - throw std::runtime_error( - "GncOptimizer::checkMuConvergence: called with unknown loss type."); - } + bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost,1e-7) < params_.relativeCostTol; + if (costConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + std::cout << "checkCostConvergence = true " << std::endl; + return costConverged; } + /// check convergence of weights to binary values + bool checkWeightsConvergence(const Vector& weights) const { + bool weightsConverged = false; + switch (params_.lossType) { + case GncParameters::GM: + weightsConverged = false; // for GM, there is no clear binary convergence for the weights + break; + case GncParameters::TLS: + weightsConverged = true; + for(size_t i=0; i params_.weightsTol ){ + weightsConverged = false; + break; + } + } + break; + default: + throw std::runtime_error( + "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); + } + if (weightsConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) + std::cout << "weightsConverged = true " << std::endl; + return weightsConverged; + } + /// check for convergence between consecutive GNC iterations bool checkConvergence(const double mu, + const Vector& weights, const double cost, const double prev_cost) const { - switch (params_.lossType) { - case GncParameters::GM: - return checkMuConvergence(mu); - case GncParameters::TLS: - return checkCostConvergence(cost, prev_cost); - default: - throw std::runtime_error( - "GncOptimizer::checkMuConvergence: called with unknown loss type."); - } + return checkCostConvergence(cost,prev_cost) || + checkWeightsConvergence(weights) || + checkMuConvergence(mu); } - /// create a graph where each factor is weighted by the gnc weights NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const { // make sure all noiseModels are Gaussian or convert to Gaussian diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index dc96762b8..9036fc97f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -66,7 +66,7 @@ TEST(GncOptimizer, gncParamsConstructor) { // change something at the gncParams level GncParams gncParams2c(gncParams2b); - gncParams2c.setLossType(GncParams::RobustLossType::TLS); + gncParams2c.setLossType(GncParams::RobustLossType::GM); CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); } @@ -186,7 +186,7 @@ TEST(GncOptimizer, updateMuTLS) { } /* ************************************************************************* */ -TEST(GncOptimizer, checkMuConvergenceGM) { +TEST(GncOptimizer, checkMuConvergence) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -194,6 +194,7 @@ TEST(GncOptimizer, checkMuConvergenceGM) { Values initial; initial.insert(X(1), p0); + { LevenbergMarquardtParams lmParams; GncParams gncParams(lmParams); gncParams.setLossType( @@ -203,6 +204,112 @@ TEST(GncOptimizer, checkMuConvergenceGM) { double mu = 1.0; CHECK(gnc.checkMuConvergence(mu)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double mu = 1.0; + CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkCostConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setRelativeCostTol(0.49); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double prev_cost = 1.0; + double cost = 0.5; + // relative cost reduction = 0.5 > 0.49, hence checkCostConvergence = false + CHECK(!gnc.checkCostConvergence(cost, prev_cost)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setRelativeCostTol(0.51); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + double prev_cost = 1.0; + double cost = 0.5; + // relative cost reduction = 0.5 < 0.51, hence checkCostConvergence = true + CHECK(gnc.checkCostConvergence(cost, prev_cost)); + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkWeightsConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::GM); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + // weights are binary, so checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + weights[0] = 0.9; // more than weightsTol = 1e-4 from 1, hence checkWeightsConvergence = false + CHECK(!gnc.checkWeightsConvergence(weights)); + } + { + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + gncParams.setWeightsTol(0.1); + auto gnc = + GncOptimizer>(fg, initial, gncParams); + + Vector weights = Vector::Ones(fg.size()); + weights[0] = 0.9; // exactly weightsTol = 0.1 from 1, hence checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); + } } /* ************************************************************************* */ @@ -455,9 +562,12 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { knownInliers.push_back(2); // nonconvexity with known inliers + { GncParams gncParams; gncParams.setKnownInliers(knownInliers); - // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + gncParams.setLossType( + GncParams::RobustLossType::GM); + //gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); auto gnc = GncOptimizer>(fg, initial, gncParams); Values gnc_result = gnc.optimize(); @@ -468,6 +578,45 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { DOUBLES_EQUAL(1.0, finalWeights[0], tol); DOUBLES_EQUAL(1.0, finalWeights[1], tol); DOUBLES_EQUAL(1.0, finalWeights[2], tol); + } + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + // gncParams.setVerbosityGNC(GncParams::VerbosityGNC::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + DOUBLES_EQUAL(0.0, finalWeights[3], tol); + } + { + // if we set the threshold large, they are all inliers + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType( + GncParams::RobustLossType::TLS); + //gncParams.setVerbosityGNC(GncParams::VerbosityGNC::VALUES); + gncParams.setInlierCostThreshold( 100.0 ); + auto gnc = GncOptimizer>(fg, initial, gncParams); + + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.25, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + DOUBLES_EQUAL(1.0, finalWeights[3], tol); + } } /* ************************************************************************* */ From 92ed225a557cfec8b58f1038ed8c6cb5b066adf7 Mon Sep 17 00:00:00 2001 From: jingnanshi Date: Thu, 24 Dec 2020 11:19:47 -0500 Subject: [PATCH 40/61] 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 b66e0f523..6637e33d4 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -66,7 +66,7 @@ public: 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 @@ public: * 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 @@ public: /// 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 @@ public: } /// 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/61] 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 3ef4dfdeb..a3923524b 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 6637e33d4..f28394545 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 @@ public: // 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: /// 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 @@ public: 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 000000000..98fb19f5f --- /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 20a8eb4c1..30e783fc9 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -42,7 +42,7 @@ public: 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 9036fc97f..f8e12fcd3 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/61] 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 98fb19f5f..30ceef4c7 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -44,7 +44,7 @@ public: }; /** Choice of robust loss function for GNC */ - enum RobustLossType { + enum GncLossType { GM /*Geman McClure*/, TLS /*Truncated least squares*/ }; @@ -61,7 +61,7 @@ public: /// GNC parameters BaseOptimizerParameters baseOptimizerParams; /*optimization parameters used to solve the weighted least squares problem at each GNC iteration*/ /// any other specific GNC parameters: - RobustLossType lossType = 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 @@ public: 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 @@ public: 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 f8e12fcd3..15a83eb4b 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/61] addressed all except 2 comments by Frank. waiting for inputs on the 2 outstanding issues --- gtsam/nonlinear/GncOptimizer.h | 232 ++++++++++---------- gtsam/nonlinear/GncParams.h | 92 ++++---- tests/testGncOptimizer.cpp | 390 ++++++++++++++++----------------- 3 files changed, 357 insertions(+), 357 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index f28394545..bbc3b9f84 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: // 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 @@ public: 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 @@ public: } // 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 @@ public: 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; } - - /// 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); + if (weightsConverged + && params_.verbosity >= GncParameters::Verbosity::SUMMARY) + std::cout << "weightsConverged = true " << std::endl; + return weightsConverged; } - /// create a graph where each factor is weighted by the gnc weights + /// 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. NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const { // make sure all noiseModels are Gaussian or convert to Gaussian NonlinearFactorGraph newGraph; @@ -287,7 +298,7 @@ public: 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 @@ public: } 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); - } - } - 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::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; - } - default: - throw std::runtime_error( - "GncOptimizer::calculateWeights: called with unknown loss type."); + 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/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 30ceef4c7..9c4f21b81 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 @@ public: * 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 @@ public: * 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 15a83eb4b..f46563b91 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); + 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)); + 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); + 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)); + 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); + 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)); + 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); + 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)); + 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); + 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)); + 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); + 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)); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); - // check weights were actually fixed: - Vector finalWeights = gnc.getWeights(); - DOUBLES_EQUAL(1.0, finalWeights[0], tol); - DOUBLES_EQUAL(1.0, finalWeights[1], tol); - DOUBLES_EQUAL(1.0, finalWeights[2], tol); + // check 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); + 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)); + 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); + // 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); + // 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)); + 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); + // 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/61] 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 bbc3b9f84..cfabf0ab6 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 9c4f21b81..5f130ddf2 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 f46563b91..738c77936 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/61] 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 cfabf0ab6..cd0b4c81a 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 5f130ddf2..0388a7fd1 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"; From b59f1bc573f5b82c4f9b2d8b82687adc9bd3beb8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 2 Jan 2021 19:24:30 -0500 Subject: [PATCH 46/61] remove build upload since it can't be used downstream --- .github/workflows/build-linux.yml | 6 ------ .github/workflows/build-macos.yml | 6 ------ .github/workflows/build-windows.yml | 6 ------ 3 files changed, 18 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 32c3bd8aa..be1da35bb 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -83,9 +83,3 @@ jobs: if: runner.os == 'Linux' run: | bash .github/scripts/unix.sh -t - - name: Upload build directory - uses: actions/upload-artifact@v2 - if: matrix.build_type == 'Release' - with: - name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} - path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index cf1a474e3..69873980a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -51,9 +51,3 @@ jobs: if: runner.os == 'macOS' run: | bash .github/scripts/unix.sh -t - - name: Upload build directory - uses: actions/upload-artifact@v2 - if: matrix.build_type == 'Release' - with: - name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} - path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 7eb908c94..887d41972 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -76,9 +76,3 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.base cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable cmake --build build --config ${{ matrix.build_type }} --target check.linear - - name: Upload build directory - uses: actions/upload-artifact@v2 - if: matrix.build_type == 'Release' - with: - name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} - path: ${{ github.workspace }}/build/ From 5333396671a1499b7535b87c37c6d40a85cd1072 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 4 Jan 2021 22:59:48 +0530 Subject: [PATCH 47/61] Adding serialization support to be used for GT-SFM (#650) * adding serialization and other functions to enable testing * adding track serialization and testable trait * improving formatting * fixing variable names and comments * adding serialization functions to wrapper * fixing xml serialization issues * reverting SfmTrack to struct * printing out the 3d point * adding equals function to wrapper * adding inline comment for round trip --- gtsam/gtsam.i | 12 ++ gtsam/slam/dataset.h | 124 +++++++++++++++++- gtsam/slam/tests/testSerializationDataset.cpp | 58 ++++++++ 3 files changed, 191 insertions(+), 3 deletions(-) create mode 100644 gtsam/slam/tests/testSerializationDataset.cpp diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d3a4973ed..e1e11964f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2768,6 +2768,12 @@ class SfmTrack { pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void add_measurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; }; class SfmData { @@ -2778,6 +2784,12 @@ class SfmData { gtsam::SfmTrack track(size_t idx) const; void add_track(const gtsam::SfmTrack& t) ; void add_camera(const gtsam::SfmCamera& cam); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 93bd2b2ee..d96c11167 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include @@ -222,7 +224,7 @@ struct SfmTrack { float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; - + /// Total number of measurements in this track size_t number_measurements() const { return measurements.size(); @@ -243,6 +245,73 @@ struct SfmTrack { void add_measurement(size_t idx, const gtsam::Point2& m) { measurements.emplace_back(idx, m); } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(p); + ar & BOOST_SERIALIZATION_NVP(r); + ar & BOOST_SERIALIZATION_NVP(g); + ar & BOOST_SERIALIZATION_NVP(b); + ar & BOOST_SERIALIZATION_NVP(measurements); + ar & BOOST_SERIALIZATION_NVP(siftIndices); + } + + /// assert equality up to a tolerance + bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const { + // check the 3D point + if (!p.isApprox(sfmTrack.p)) { + return false; + } + + // check the RGB values + if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) { + return false; + } + + // compare size of vectors for measurements and siftIndices + if (number_measurements() != sfmTrack.number_measurements() || + siftIndices.size() != sfmTrack.siftIndices.size()) { + return false; + } + + // compare measurements (order sensitive) + for (size_t idx = 0; idx < number_measurements(); ++idx) { + SfmMeasurement measurement = measurements[idx]; + SfmMeasurement otherMeasurement = sfmTrack.measurements[idx]; + + if (measurement.first != otherMeasurement.first || + !measurement.second.isApprox(otherMeasurement.second)) { + return false; + } + } + + // compare sift indices (order sensitive) + for (size_t idx = 0; idx < siftIndices.size(); ++idx) { + SiftIndex index = siftIndices[idx]; + SiftIndex otherIndex = sfmTrack.siftIndices[idx]; + + if (index.first != otherIndex.first || + index.second != otherIndex.second) { + return false; + } + } + + return true; + } + + /// print + void print(const std::string& s = "") const { + cout << "Track with " << measurements.size(); + cout << " measurements of point " << p << "\n"; + } +}; + +/* ************************************************************************* */ +/// traits +template<> +struct traits : public Testable { }; @@ -269,13 +338,62 @@ struct SfmData { return tracks[idx]; } /// Add a track to SfmData - void add_track(const SfmTrack& t) { + void add_track(const SfmTrack& t) { tracks.push_back(t); } /// Add a camera to SfmData - void add_camera(const SfmCamera& cam){ + void add_camera(const SfmCamera& cam) { cameras.push_back(cam); } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(cameras); + ar & BOOST_SERIALIZATION_NVP(tracks); + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const SfmData &sfmData, double tol = 1e-9) const { + // check number of cameras and tracks + if (number_cameras() != sfmData.number_cameras() || + number_tracks() != sfmData.number_tracks()) { + return false; + } + + // check each camera + for (size_t i = 0; i < number_cameras(); ++i) { + if (!camera(i).equals(sfmData.camera(i), tol)) { + return false; + } + } + + // check each track + for (size_t j = 0; j < number_tracks(); ++j) { + if (!track(j).equals(sfmData.track(j), tol)) { + return false; + } + } + + return true; + } + + /// print + void print(const std::string& s = "") const { + cout << "Number of cameras = " << number_cameras() << "\n"; + cout << "Number of tracks = " << number_tracks() << "\n"; + } +}; + +/* ************************************************************************* */ +/// traits +template<> +struct traits : public Testable { }; /** diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp new file mode 100644 index 000000000..6ef82f07f --- /dev/null +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationDataset.cpp + * @brief serialization tests for dataset.cpp + * @author Ayush Baid + * @date Jan 1, 2021 + */ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST(dataSet, sfmDataSerialization) { + // Test the serialization of SfmData + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData mydata; + CHECK(readBAL(filename, mydata)); + + // round-trip equality check on serialization and subsequent deserialization + EXPECT(equalsObj(mydata)); + EXPECT(equalsXML(mydata)); + EXPECT(equalsBinary(mydata)); +} + +/* ************************************************************************* */ +TEST(dataSet, sfmTrackSerialization) { + // Test the serialization of SfmTrack + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData mydata; + CHECK(readBAL(filename, mydata)); + + SfmTrack track = mydata.track(0); + + // round-trip equality check on serialization and subsequent deserialization + EXPECT(equalsObj(track)); + EXPECT(equalsXML(track)); + EXPECT(equalsBinary(track)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From 7cc7232a990fab581506a79d15c6108b491d33f0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jan 2021 13:11:36 -0500 Subject: [PATCH 48/61] Squashed 'wrap/' changes from dfa624e77..09f8bbf71 09f8bbf71 Merge pull request #25 from borglab/fix/function-name 0dbfb6c13 fix function name to be the correct one f69f8b01f Merge pull request #24 from borglab/fix/pip 6519a6627 use pip install to overcome superuser issues b11ecf4e8 Merge pull request #23 from borglab/fix/remove-pip-args 813030108 remove pip-args since we are using setup.py 498d233e0 Merge pull request #22 from borglab/fix/package-install 846212ac3 set correct flags for installing gtwrap package 62161cd20 Merge pull request #21 from borglab/feature/script-vars 93be1d9f8 set script variables and move pybind11 loading so gtwrap can be used under gtsam 8770e3c7e Merge pull request #20 from borglab/fix/pybind-include 8c3c83618 proper placement of pybind11 include a9ad4f504 Merge pull request #19 from borglab/feature/package 99d8a12c7 added more documentation 4cbec1579 change to macro so we don't have to deal with function scopes b83e405b8 updates to completely install the package 38a64b3de new scripts which will be installed to bin directory bf9646235 Merge pull request #18 from borglab/fix/cmake-min c7c280099 Consistent cmake minimum required 42df58f62 Merge pull request #17 from borglab/fix/cleanup e580b282d version bump 4ccd66fa5 More finegrained handling of Python version 6476fd710 Merge pull request #16 from borglab/feature/better-find-python 8ac1296a0 use setup.py to install dependencies e9ac473be install dependencies and support versions of CMake<3.12 cf272dbd2 Merge pull request #15 from borglab/feature/utils ffc9cc4f7 new utils to reduce boilerplate 20e8e8b7a Merge pull request #11 from borglab/feature/package 04b844bd6 use new version of FindPython and be consistent 3f9d7a32a Merge pull request #13 from borglab/add_license c791075a6 Add LICENSE 517b67c46 correct working directory for setup.py 1b22b47ae move matlab.h to root directory 37b407214 Proper source directory path for use in other projects 61696dd5d configure PybindWrap within the cmake directory 1b91fc9af add config file so we can use find_package a1e6f4f53 small typo da9f351be updated README and housekeeping 64b8f78d5 files needed to allow for packaging bddda7f54 package structure git-subtree-dir: wrap git-subtree-split: 09f8bbf7172ba8b1bd3d2484795743f16e1a5893 --- .gitignore | 3 + CMakeLists.txt | 55 +++++++++++ LICENSE | 13 +++ README.md | 66 ++++++------- cmake/GtwrapUtils.cmake | 74 +++++++++++++++ cmake/PybindWrap.cmake | 35 +------ cmake/gtwrapConfig.cmake | 27 ++++++ gtwrap/__init__.py | 0 .../interface_parser.py | 0 matlab_wrapper.py => gtwrap/matlab_wrapper.py | 58 +----------- pybind_wrapper.py => gtwrap/pybind_wrapper.py | 78 +--------------- .../template_instantiator.py | 2 +- scripts/matlab_wrap.py | 68 ++++++++++++++ scripts/pybind_wrap.py | 93 +++++++++++++++++++ setup.py | 36 +++++++ tests/interface_parser_test.py | 2 +- tests/test_matlab_wrapper.py | 12 +-- tests/test_pybind_wrapper.py | 6 +- 18 files changed, 424 insertions(+), 204 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 cmake/GtwrapUtils.cmake create mode 100644 cmake/gtwrapConfig.cmake create mode 100644 gtwrap/__init__.py rename interface_parser.py => gtwrap/interface_parser.py (100%) rename matlab_wrapper.py => gtwrap/matlab_wrapper.py (96%) rename pybind_wrapper.py => gtwrap/pybind_wrapper.py (84%) rename template_instantiator.py => gtwrap/template_instantiator.py (99%) create mode 100644 scripts/matlab_wrap.py create mode 100644 scripts/pybind_wrap.py create mode 100644 setup.py diff --git a/.gitignore b/.gitignore index 38da6d9d1..4fd660b95 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ __pycache__/ .vscode/ +*build* +*dist* +*.egg-info diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..4c89ab96e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.9) + +# Set the project name and version +project(GTwrap VERSION 1.0) + +# ############################################################################## +# General configuration + +set(WRAP_PYTHON_VERSION + "Default" + CACHE STRING "The Python version to use for wrapping") + +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/GtwrapUtils.cmake) +gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) + +# ############################################################################## +# Install the CMake file to be used by other projects +if(WIN32 AND NOT CYGWIN) + set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake") +else() + set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake") +endif() + +# Install scripts to the standard CMake script directory. +install(FILES cmake/gtwrapConfig.cmake cmake/PybindWrap.cmake + cmake/GtwrapUtils.cmake + DESTINATION "${SCRIPT_INSTALL_DIR}/gtwrap") + +# Install wrapping scripts as binaries to `CMAKE_INSTALL_PREFIX/bin` so they can +# be invoked for wrapping. +install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py TYPE BIN) + +# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/pybind11` This will +# allow the gtwrapConfig.cmake file to load it later. +install(DIRECTORY pybind11 TYPE LIB) + +# ############################################################################## +# Install the Python package +find_package( + Python ${WRAP_PYTHON_VERSION} + COMPONENTS Interpreter + REQUIRED) + +# Detect virtualenv and set Pip args accordingly +# https://www.scivision.dev/cmake-install-python-package/ +if(DEFINED ENV{VIRTUAL_ENV} OR DEFINED ENV{CONDA_PREFIX}) + set(_pip_args) +else() + set(_pip_args "--user") +endif() +#TODO add correct flags for virtualenv + +# Finally install the gtwrap python package. +execute_process(COMMAND ${Python_EXECUTABLE} -m pip install . ${_pip_args} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/LICENSE b/LICENSE new file mode 100644 index 000000000..406b266b7 --- /dev/null +++ b/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2010, Georgia Tech Research Corporation +Atlanta, Georgia 30332-0415 +All Rights Reserved + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/README.md b/README.md index f72c3f652..347cca601 100644 --- a/README.md +++ b/README.md @@ -8,9 +8,40 @@ It was designed to be more general than just wrapping GTSAM. For notes on creati 1. This library uses `pybind11`, which is included as a subdirectory in GTSAM. 2. The `interface_parser.py` in this library uses `pyparsing` to parse the interface file `gtsam.h`. Please install it first in your current Python environment before attempting the build. - ``` - python3 -m pip install pyparsing - ``` + +``` +python3 -m pip install pyparsing +``` + +## Getting Started + +Clone this repository to your local machine and perform the standard CMake install: + +```sh +mkdir build && cd build +cmake .. +make install # use sudo if needed +``` + +Using `wrap` in your project is straightforward from here. In you `CMakeLists.txt` file, you just need to add the following: + +```cmake +include(PybindWrap) + +pybind_wrap(${PROJECT_NAME}_py # target + ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file + "${PROJECT_NAME}.cpp" # the generated cpp + "${PROJECT_NAME}" # module_name + "gtsam" # top namespace in the cpp file + "${ignore}" # ignore classes + ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl + ${PROJECT_NAME} # libs + "${PROJECT_NAME}" # dependencies + ON # use boost + ) +``` + +For more information, please follow our [tutorial](https://github.com/borglab/gtsam-project-python). ## GTSAM Python wrapper @@ -45,32 +76,3 @@ It was designed to be more general than just wrapping GTSAM. For notes on creati python setup.py install ``` - NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages. - - -## Old GTSAM Wrapper - -*Outdated note from the original wrap.* - -TODO: Update this. - -It was designed to be more general than just wrapping GTSAM, but a small amount of GTSAM specific code exists in `matlab.h`, the include file that is included by the `mex` files. The GTSAM-specific functionality consists primarily of handling of Eigen Matrix and Vector classes. - -For notes on creating a wrap interface, see `gtsam.h` for what features can be wrapped into a toolbox, as well as the current state of the toolbox for GTSAM. For more technical details on the interface, please read comments in `matlab.h` - -Some good things to know: - -OBJECT CREATION - -- Classes are created by special constructors, e.g., `new_GaussianFactorGraph_.cpp`. - These constructors are called from the MATLAB class `@GaussianFactorGraph`. - `new_GaussianFactorGraph_` calls wrap_constructed in `matlab.h`, see documentation there - -METHOD (AND CONSTRUCTOR) ARGUMENTS - -- Simple argument types of methods, such as "double", will be converted in the - `mex` wrappers by calling unwrap, defined in matlab.h -- Vector and Matrix arguments are normally passed by reference in GTSAM, but - in `gtsam.h` you need to pretend they are passed by value, to trigger the - generation of the correct conversion routines `unwrap` and `unwrap` -- passing classes as arguments works, provided they are passed by reference. - This triggers a call to unwrap_shared_ptr diff --git a/cmake/GtwrapUtils.cmake b/cmake/GtwrapUtils.cmake new file mode 100644 index 000000000..9c6b141a0 --- /dev/null +++ b/cmake/GtwrapUtils.cmake @@ -0,0 +1,74 @@ +# Utilities to help with wrapping. + +macro(get_python_version) + if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + # Use older version of cmake's find_python + find_package(PythonInterp) + + if(NOT ${PYTHONINTERP_FOUND}) + message( + FATAL_ERROR + "Cannot find Python interpreter. Please install Python >= 3.6.") + endif() + + find_package(PythonLibs ${PYTHON_VERSION_STRING}) + + set(Python_VERSION_MAJOR + ${PYTHON_VERSION_MAJOR} + PARENT_SCOPE) + set(Python_VERSION_MINOR + ${PYTHON_VERSION_MINOR} + PARENT_SCOPE) + set(Python_VERSION_PATCH + ${PYTHON_VERSION_PATCH} + PARENT_SCOPE) + set(Python_EXECUTABLE + ${PYTHON_EXECUTABLE} + PARENT_SCOPE) + + else() + # Get info about the Python interpreter + # https://cmake.org/cmake/help/latest/module/FindPython.html#module:FindPython + find_package(Python COMPONENTS Interpreter Development) + + if(NOT ${Python_FOUND}) + message( + FATAL_ERROR + "Cannot find Python interpreter. Please install Python>=3.6.") + endif() + + endif() +endmacro() + +# Set the Python version for the wrapper and set the paths to the executable and +# include/library directories. WRAP_PYTHON_VERSION can be "Default" or a +# specific major.minor version. +macro(gtwrap_get_python_version WRAP_PYTHON_VERSION) + # Unset these cached variables to avoid surprises when the python in the + # current environment are different from the cached! + unset(Python_EXECUTABLE CACHE) + unset(Python_INCLUDE_DIRS CACHE) + unset(Python_VERSION_MAJOR CACHE) + unset(Python_VERSION_MINOR CACHE) + unset(Python_VERSION_PATCH CACHE) + + # Allow override + if(${WRAP_PYTHON_VERSION} STREQUAL "Default") + # Check for Python3 or Python2 in order + get_python_version() + + # Set the wrapper python version + set(WRAP_PYTHON_VERSION + "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}.${Python_VERSION_PATCH}" + CACHE STRING "The version of Python to build the wrappers against." + FORCE) + + else() + # Find the Python that best matches the python version specified. + find_package( + Python ${WRAP_PYTHON_VERSION} + COMPONENTS Interpreter Development + EXACT REQUIRED) + endif() + +endmacro() diff --git a/cmake/PybindWrap.cmake b/cmake/PybindWrap.cmake index 85f956d50..a94dbc5cc 100644 --- a/cmake/PybindWrap.cmake +++ b/cmake/PybindWrap.cmake @@ -1,32 +1,5 @@ -# Unset these cached variables to avoid surprises when the python in the current -# environment are different from the cached! -unset(PYTHON_EXECUTABLE CACHE) -unset(PYTHON_INCLUDE_DIR CACHE) -unset(PYTHON_MAJOR_VERSION CACHE) - -# Allow override from command line -if(NOT DEFINED WRAP_USE_CUSTOM_PYTHON_LIBRARY) - if(WRAP_PYTHON_VERSION STREQUAL "Default") - find_package(PythonInterp REQUIRED) - find_package(PythonLibs REQUIRED) - else() - find_package(PythonInterp - ${WRAP_PYTHON_VERSION} - EXACT - REQUIRED) - find_package(PythonLibs - ${WRAP_PYTHON_VERSION} - EXACT - REQUIRED) - endif() -endif() - -set(DIR_OF_WRAP_PYBIND_CMAKE ${CMAKE_CURRENT_LIST_DIR}) - set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) -add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 pybind11) - # User-friendly Pybind11 wrapping and installing function. # Builds a Pybind11 module from the provided interface_header. # For example, for the interface header gtsam.h, this will @@ -65,7 +38,7 @@ function(pybind_wrap add_custom_command(OUTPUT ${generated_cpp} COMMAND ${PYTHON_EXECUTABLE} - ${CMAKE_SOURCE_DIR}/wrap/pybind_wrapper.py + ${PYBIND_WRAP_SCRIPT} --src ${interface_header} --out @@ -89,9 +62,9 @@ function(pybind_wrap # ~~~ add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} - ${CMAKE_SOURCE_DIR}/wrap/interface_parser.py - ${CMAKE_SOURCE_DIR}/wrap/pybind_wrapper.py - ${CMAKE_SOURCE_DIR}/wrap/template_instantiator.py + # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py + # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py + # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py APPEND) pybind11_add_module(${target} ${generated_cpp}) diff --git a/cmake/gtwrapConfig.cmake b/cmake/gtwrapConfig.cmake new file mode 100644 index 000000000..48bd4772d --- /dev/null +++ b/cmake/gtwrapConfig.cmake @@ -0,0 +1,27 @@ +# This config file modifies CMAKE_MODULE_PATH so that the wrap cmake files may +# be included This file also allows the use of `find_package(gtwrap)` in CMake. + +set(GTWRAP_DIR "${CMAKE_CURRENT_LIST_DIR}") +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}") + +if(WIN32 AND NOT CYGWIN) + set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake") +else() + set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake") +endif() + +# Standard includes +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) +include(CMakeDependentOption) + +# Load all the CMake scripts from the standard location +include(${SCRIPT_INSTALL_DIR}/gtwrap/PybindWrap.cmake) +include(${SCRIPT_INSTALL_DIR}/gtwrap/GtwrapUtils.cmake) + +# Set the variables for the wrapping scripts to be used in the build. +set(PYBIND_WRAP_SCRIPT "${CMAKE_INSTALL_FULL_BINDIR}/pybind_wrap.py") +set(MATLAB_WRAP_SCRIPT "${CMAKE_INSTALL_FULL_BINDIR}/matlab_wrap.py") + +# Load the pybind11 code from the library installation path +add_subdirectory(${CMAKE_INSTALL_FULL_LIBDIR}/pybind11 pybind11) diff --git a/gtwrap/__init__.py b/gtwrap/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/interface_parser.py b/gtwrap/interface_parser.py similarity index 100% rename from interface_parser.py rename to gtwrap/interface_parser.py diff --git a/matlab_wrapper.py b/gtwrap/matlab_wrapper.py similarity index 96% rename from matlab_wrapper.py rename to gtwrap/matlab_wrapper.py index 1b6b75a49..355913ba7 100755 --- a/matlab_wrapper.py +++ b/gtwrap/matlab_wrapper.py @@ -2,8 +2,8 @@ import os import argparse import textwrap -import interface_parser as parser -import template_instantiator as instantiator +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator from functools import reduce from functools import partial @@ -1666,7 +1666,7 @@ class MatlabWrapper(object): return self.content -def _generate_content(cc_content, path, verbose=False): +def generate_content(cc_content, path, verbose=False): """Generate files and folders from matlab wrapper content. Keyword arguments: @@ -1698,7 +1698,7 @@ def _generate_content(cc_content, path, verbose=False): for sub_content in c: import sys _debug("sub object: {}".format(sub_content[1][0][0])) - _generate_content(sub_content[1], path_to_folder) + generate_content(sub_content[1], path_to_folder) elif type(c[1]) == list: path_to_folder = path + '/' + c[0] @@ -1726,53 +1726,3 @@ def _generate_content(cc_content, path, verbose=False): with open(path_to_file, 'w') as f: f.write(c[1]) - - -if __name__ == "__main__": - arg_parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument("--src", type=str, required=True, help="Input interface .h file.") - arg_parser.add_argument("--module_name", type=str, required=True, help="Name of the C++ class being wrapped.") - arg_parser.add_argument("--out", type=str, required=True, help="Name of the output folder.") - arg_parser.add_argument("--top_module_namespaces", - type=str, - default="", - help="C++ namespace for the top module, e.g. `ns1::ns2::ns3`. " - "Only the content within this namespace and its sub-namespaces " - "will be wrapped. The content of this namespace will be available at " - "the top module level, and its sub-namespaces' in the submodules.\n" - "For example, `import ` gives you access to a Python " - "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" - ", and `from import ns4` gives you access to a Python " - "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ") - arg_parser.add_argument("--ignore", - nargs='*', - type=str, - help="A space-separated list of classes to ignore. " - "Class names must include their full namespaces.") - args = arg_parser.parse_args() - - top_module_namespaces = args.top_module_namespaces.split("::") - if top_module_namespaces[0]: - top_module_namespaces = [''] + top_module_namespaces - - with open(args.src, 'r') as f: - content = f.read() - - if not os.path.exists(args.src): - os.mkdir(args.src) - - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - - import sys - - print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) - wrapper = MatlabWrapper(module=module, - module_name=args.module_name, - top_module_namespace=top_module_namespaces, - ignore_classes=args.ignore) - - cc_content = wrapper.wrap() - - _generate_content(cc_content, args.out) diff --git a/pybind_wrapper.py b/gtwrap/pybind_wrapper.py similarity index 84% rename from pybind_wrapper.py rename to gtwrap/pybind_wrapper.py index 326d9be52..c0e88e37a 100755 --- a/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -9,12 +9,11 @@ See LICENSE for the license information Code generator for wrapping a C++ module with Pybind11 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert """ -import argparse import re import textwrap -import interface_parser as parser -import template_instantiator as instantiator +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator class PybindWrapper(object): @@ -319,76 +318,3 @@ class PybindWrapper(object): wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, ) - - -def main(): - arg_parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument("--src", type=str, required=True, help="Input interface .h file") - arg_parser.add_argument( - "--module_name", - type=str, - required=True, - help="Name of the Python module to be generated and " - "used in the Python `import` statement.", - ) - arg_parser.add_argument( - "--out", - type=str, - required=True, - help="Name of the output pybind .cc file", - ) - arg_parser.add_argument( - "--use-boost", - action="store_true", - help="using boost's shared_ptr instead of std's", - ) - arg_parser.add_argument( - "--top_module_namespaces", - type=str, - default="", - help="C++ namespace for the top module, e.g. `ns1::ns2::ns3`. " - "Only the content within this namespace and its sub-namespaces " - "will be wrapped. The content of this namespace will be available at " - "the top module level, and its sub-namespaces' in the submodules.\n" - "For example, `import ` gives you access to a Python " - "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" - "and `from import ns4` gives you access to a Python " - "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ", - ) - arg_parser.add_argument( - "--ignore", - nargs='*', - type=str, - help="A space-separated list of classes to ignore. " - "Class names must include their full namespaces.", - ) - arg_parser.add_argument("--template", type=str, help="The module template file") - args = arg_parser.parse_args() - - top_module_namespaces = args.top_module_namespaces.split("::") - if top_module_namespaces[0]: - top_module_namespaces = [''] + top_module_namespaces - - with open(args.src, "r") as f: - content = f.read() - module = parser.Module.parseString(content) - instantiator.instantiate_namespace_inplace(module) - - with open(args.template, "r") as f: - template_content = f.read() - wrapper = PybindWrapper( - module=module, - module_name=args.module_name, - use_boost=args.use_boost, - top_module_namespaces=top_module_namespaces, - ignore_classes=args.ignore, - module_template=template_content, - ) - - cc_content = wrapper.wrap() - with open(args.out, "w") as f: - f.write(cc_content) - - -if __name__ == "__main__": - main() diff --git a/template_instantiator.py b/gtwrap/template_instantiator.py similarity index 99% rename from template_instantiator.py rename to gtwrap/template_instantiator.py index 3d98e9699..6032beac4 100644 --- a/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -1,4 +1,4 @@ -import interface_parser as parser +import gtwrap.interface_parser as parser def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, instantiated_class=None): diff --git a/scripts/matlab_wrap.py b/scripts/matlab_wrap.py new file mode 100644 index 000000000..232e93490 --- /dev/null +++ b/scripts/matlab_wrap.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +""" +Helper script to wrap C++ to Matlab. +This script is installed via CMake to the user's binary directory +and invoked during the wrapping by CMake. +""" + +import argparse +import os + +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator +from gtwrap.matlab_wrapper import MatlabWrapper, generate_content + +if __name__ == "__main__": + arg_parser = argparse.ArgumentParser( + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + arg_parser.add_argument("--src", type=str, required=True, + help="Input interface .h file.") + arg_parser.add_argument("--module_name", type=str, required=True, + help="Name of the C++ class being wrapped.") + arg_parser.add_argument("--out", type=str, required=True, + help="Name of the output folder.") + arg_parser.add_argument( + "--top_module_namespaces", + type=str, + default="", + help="C++ namespace for the top module, e.g. `ns1::ns2::ns3`. " + "Only the content within this namespace and its sub-namespaces " + "will be wrapped. The content of this namespace will be available at " + "the top module level, and its sub-namespaces' in the submodules.\n" + "For example, `import ` gives you access to a Python " + "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" + ", and `from import ns4` gives you access to a Python " + "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ") + arg_parser.add_argument("--ignore", + nargs='*', + type=str, + help="A space-separated list of classes to ignore. " + "Class names must include their full namespaces.") + args = arg_parser.parse_args() + + top_module_namespaces = args.top_module_namespaces.split("::") + if top_module_namespaces[0]: + top_module_namespaces = [''] + top_module_namespaces + + with open(args.src, 'r') as f: + content = f.read() + + if not os.path.exists(args.src): + os.mkdir(args.src) + + module = parser.Module.parseString(content) + + instantiator.instantiate_namespace_inplace(module) + + import sys + + print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) + wrapper = MatlabWrapper(module=module, + module_name=args.module_name, + top_module_namespace=top_module_namespaces, + ignore_classes=args.ignore) + + cc_content = wrapper.wrap() + + generate_content(cc_content, args.out) diff --git a/scripts/pybind_wrap.py b/scripts/pybind_wrap.py new file mode 100644 index 000000000..e641cfaaf --- /dev/null +++ b/scripts/pybind_wrap.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 + +""" +Helper script to wrap C++ to Python with Pybind. +This script is installed via CMake to the user's binary directory +and invoked during the wrapping by CMake. +""" + +import argparse + +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator +from gtwrap.pybind_wrapper import PybindWrapper + + +def main(): + """Main runner.""" + arg_parser = argparse.ArgumentParser( + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + arg_parser.add_argument( + "--src", + type=str, + required=True, + help="Input interface .i/.h file") + arg_parser.add_argument( + "--module_name", + type=str, + required=True, + help="Name of the Python module to be generated and " + "used in the Python `import` statement.", + ) + arg_parser.add_argument( + "--out", + type=str, + required=True, + help="Name of the output pybind .cc file", + ) + arg_parser.add_argument( + "--use-boost", + action="store_true", + help="using boost's shared_ptr instead of std's", + ) + arg_parser.add_argument( + "--top_module_namespaces", + type=str, + default="", + help="C++ namespace for the top module, e.g. `ns1::ns2::ns3`. " + "Only the content within this namespace and its sub-namespaces " + "will be wrapped. The content of this namespace will be available at " + "the top module level, and its sub-namespaces' in the submodules.\n" + "For example, `import ` gives you access to a Python " + "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" + "and `from import ns4` gives you access to a Python " + "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ", + ) + arg_parser.add_argument( + "--ignore", + nargs='*', + type=str, + help="A space-separated list of classes to ignore. " + "Class names must include their full namespaces.", + ) + arg_parser.add_argument("--template", type=str, + help="The module template file") + args = arg_parser.parse_args() + + top_module_namespaces = args.top_module_namespaces.split("::") + if top_module_namespaces[0]: + top_module_namespaces = [''] + top_module_namespaces + + with open(args.src, "r") as f: + content = f.read() + module = parser.Module.parseString(content) + instantiator.instantiate_namespace_inplace(module) + + with open(args.template, "r") as f: + template_content = f.read() + wrapper = PybindWrapper( + module=module, + module_name=args.module_name, + use_boost=args.use_boost, + top_module_namespaces=top_module_namespaces, + ignore_classes=args.ignore, + module_template=template_content, + ) + + cc_content = wrapper.wrap() + with open(args.out, "w") as f: + f.write(cc_content) + + +if __name__ == "__main__": + main() diff --git a/setup.py b/setup.py new file mode 100644 index 000000000..10fc53d34 --- /dev/null +++ b/setup.py @@ -0,0 +1,36 @@ +"""Setup file for the GTwrap package""" + +try: + from setuptools import find_packages, setup +except ImportError: + from distutils.core import find_packages, setup + +packages = find_packages() + +setup( + name='gtwrap', + description='Library to wrap C++ with Python and Matlab', + version='1.1.0', + author="Frank Dellaert et. al.", + author_email="dellaert@gatech.edu", + license='BSD', + keywords="wrap, bindings, cpp, python", + long_description=open("README.md").read(), + long_description_content_type="text/markdown", + python_requires=">=3.6", + # https://pypi.org/classifiers + classifiers=[ + 'Development Status :: 4 - Beta', + 'Intended Audience :: Education', + 'Intended Audience :: Developers', + 'Intended Audience :: Science/Research', + 'Operating System :: MacOS', + 'Operating System :: Microsoft :: Windows', + 'Operating System :: POSIX', + 'Programming Language :: Python :: 3', + 'Topic :: Software Development :: Libraries' + ], + packages=packages, + platforms="any", + install_requires=open("requirements.txt").readlines(), +) diff --git a/tests/interface_parser_test.py b/tests/interface_parser_test.py index 3e989a519..3197b4214 100644 --- a/tests/interface_parser_test.py +++ b/tests/interface_parser_test.py @@ -4,7 +4,7 @@ import unittest import sys, os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from interface_parser import * +from gtwrap.interface_parser import * class TestPyparsing(unittest.TestCase): diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index 18dc49baf..258f2da8f 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -10,9 +10,9 @@ import filecmp sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -import template_instantiator as instantiator -import interface_parser as parser -from matlab_wrapper import MatlabWrapper +import gtwrap.template_instantiator as instantiator +import gtwrap.interface_parser as parser +from gtwrap.matlab_wrapper import MatlabWrapper class TestWrap(unittest.TestCase): @@ -20,7 +20,7 @@ class TestWrap(unittest.TestCase): MATLAB_TEST_DIR = TEST_DIR + "expected-matlab/" MATLAB_ACTUAL_DIR = TEST_DIR + "actual-matlab/" - def _generate_content(self, cc_content, path=''): + def generate_content(self, cc_content, path=''): """Generate files and folders from matlab wrapper content. Keyword arguments: @@ -48,7 +48,7 @@ class TestWrap(unittest.TestCase): for sub_content in c: import sys print("sub object: {}".format(sub_content[1][0][0]), file=sys.stderr) - self._generate_content(sub_content[1], path_to_folder) + self.generate_content(sub_content[1], path_to_folder) elif type(c[1]) == list: path_to_folder = path + '/' + c[0] @@ -104,7 +104,7 @@ class TestWrap(unittest.TestCase): cc_content = wrapper.wrap() - self._generate_content(cc_content) + self.generate_content(cc_content) def compare_and_diff(file): output = self.MATLAB_ACTUAL_DIR + file diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index 80032cbcd..d859cc99f 100644 --- a/tests/test_pybind_wrapper.py +++ b/tests/test_pybind_wrapper.py @@ -14,9 +14,9 @@ import os.path as path sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(os.path.normpath(os.path.abspath(os.path.join(__file__, '../../../build/wrap')))) -from pybind_wrapper import PybindWrapper -import interface_parser as parser -import template_instantiator as instantiator +from gtwrap.pybind_wrapper import PybindWrapper +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) From 3a80b38a9a5c2a86b62b3a14d657cc173d9624a7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jan 2021 13:13:01 -0500 Subject: [PATCH 49/61] updates to Cmake to use the new wrap package --- CMakeLists.txt | 5 ++ cmake/GtsamMatlabWrap.cmake | 8 ++- cmake/HandlePython.cmake | 50 ++++++++++++---- python/CMakeLists.txt | 115 +++++++++++++++++++++--------------- 4 files changed, 116 insertions(+), 62 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 35c487fd3..0c39089c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,11 @@ add_subdirectory(CppUnitLite) # This is the new wrapper if(GTSAM_BUILD_PYTHON) + # Need to set this for the wrap package so we don't use the default value. + set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} + CACHE STRING "The Python version to use for wrapping") + + add_subdirectory(wrap) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") add_subdirectory(python) endif() diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index b17618f49..b76f96a4e 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -240,12 +240,16 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex set(_ignore gtsam::Point2 gtsam::Point3) - add_custom_command( + + # set the matlab wrapping script variable + set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") + + add_custom_command( OUTPUT ${generated_cpp_file} DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects} COMMAND ${PYTHON_EXECUTABLE} - ${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py + ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader} --module_name ${moduleName} --out ${generated_files_path} diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake index e5d55b451..0c24824bc 100644 --- a/cmake/HandlePython.cmake +++ b/cmake/HandlePython.cmake @@ -1,22 +1,48 @@ # Set Python version if either Python or MATLAB wrapper is requested. if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) - if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") - # Get info about the Python3 interpreter - # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 - find_package(Python3 COMPONENTS Interpreter Development) + if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") - if(NOT ${Python3_FOUND}) - message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.") - endif() + if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + # Use older version of cmake's find_python + find_package(PythonInterp) + + if(NOT ${PYTHONINTERP_FOUND}) + message( + FATAL_ERROR + "Cannot find Python interpreter. Please install Python >= 3.6.") + endif() + + find_package(PythonLibs ${PYTHON_VERSION_STRING}) + + set(Python_VERSION_MAJOR ${PYTHON_VERSION_MAJOR}) + set(Python_VERSION_MINOR ${PYTHON_VERSION_MINOR}) + set(Python_EXECUTABLE ${PYTHON_EXECUTABLE}) + + else() + # Get info about the Python3 interpreter + # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 + find_package(Python3 COMPONENTS Interpreter Development) + + if(NOT ${Python3_FOUND}) + message( + FATAL_ERROR + "Cannot find Python3 interpreter. Please install Python >= 3.6.") + endif() + + set(Python_VERSION_MAJOR ${Python3_VERSION_MAJOR}) + set(Python_VERSION_MINOR ${Python3_VERSION_MINOR}) - set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" - CACHE - STRING - "The version of Python to build the wrappers against." - FORCE) endif() + + set(GTSAM_PYTHON_VERSION + "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}" + CACHE STRING "The version of Python to build the wrappers against." + FORCE) + + endif() endif() +# Check for build of Unstable modules if(GTSAM_BUILD_PYTHON) if(GTSAM_UNSTABLE_BUILD_PYTHON) if (NOT GTSAM_BUILD_UNSTABLE) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bfe08a76a..b50701464 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -4,20 +4,26 @@ if (NOT GTSAM_BUILD_PYTHON) return() endif() -# Common directory for storing data/datasets stored with the package. -# This will store the data in the Python site package directly. -set(GTSAM_PYTHON_DATASET_DIR "./gtsam/Data") - # Generate setup.py. file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py) -set(WRAP_USE_CUSTOM_PYTHON_LIBRARY ${GTSAM_USE_CUSTOM_PYTHON_LIBRARY}) -set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}) +# Supply MANIFEST.in for older versions of Python +file(COPY ${PROJECT_SOURCE_DIR}/python/MANIFEST.in + DESTINATION ${GTSAM_PYTHON_BUILD_DIRECTORY}) include(PybindWrap) +############################################################ +## Load the necessary files to compile the wrapper + +# Load the pybind11 code +add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) +# Set the wrapping script variable +set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") +############################################################ + add_custom_target(gtsam_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam/gtsam.i") add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i") @@ -67,55 +73,68 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") +# Common directory for data/datasets stored with the package. +# This will store the data in the Python site package directly. +file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") + +# Add gtsam as a dependency to the install target +set(GTSAM_PYTHON_DEPENDENCIES gtsam_py) + + if(GTSAM_UNSTABLE_BUILD_PYTHON) -set(ignore - gtsam::Point2 - gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix - gtsam::ISAM2ThresholdMapValue - gtsam::FactorIndices - gtsam::FactorIndexSet - gtsam::BetweenFactorPose3s - gtsam::Point2Vector - gtsam::Pose3Vector - gtsam::KeyVector - gtsam::FixedLagSmootherKeyTimestampMapValue - gtsam::BinaryMeasurementsUnit3 - gtsam::CameraSetCal3_S2 - gtsam::CameraSetCal3Bundler - gtsam::KeyPairDoubleMap) - -pybind_wrap(gtsam_unstable_py # target - ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header - "gtsam_unstable.cpp" # generated_cpp - "gtsam_unstable" # module_name - "gtsam" # top_namespace - "${ignore}" # ignore_classes - ${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl - gtsam_unstable # libs - "gtsam_unstable;gtsam_unstable_header" # dependencies - ON # use_boost - ) + set(ignore + gtsam::Point2 + gtsam::Point3 + gtsam::LieVector + gtsam::LieMatrix + gtsam::ISAM2ThresholdMapValue + gtsam::FactorIndices + gtsam::FactorIndexSet + gtsam::BetweenFactorPose3s + gtsam::Point2Vector + gtsam::Pose3Vector + gtsam::KeyVector + gtsam::FixedLagSmootherKeyTimestampMapValue + gtsam::BinaryMeasurementsUnit3 + gtsam::CameraSetCal3_S2 + gtsam::CameraSetCal3Bundler + gtsam::KeyPairDoubleMap) + + pybind_wrap(gtsam_unstable_py # target + ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header + "gtsam_unstable.cpp" # generated_cpp + "gtsam_unstable" # module_name + "gtsam" # top_namespace + "${ignore}" # ignore_classes + ${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl + gtsam_unstable # libs + "gtsam_unstable;gtsam_unstable_header" # dependencies + ON # use_boost + ) -set_target_properties(gtsam_unstable_py PROPERTIES - INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" - INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam_unstable" - LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable" - DEBUG_POSTFIX "" # Otherwise you will have a wrong name - RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name - ) + set_target_properties(gtsam_unstable_py PROPERTIES + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" + INSTALL_RPATH_USE_LINK_PATH TRUE + OUTPUT_NAME "gtsam_unstable" + LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable" + DEBUG_POSTFIX "" # Otherwise you will have a wrong name + RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name + ) -set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) + set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) + + # Symlink all tests .py files to build folder. + create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" + "${GTSAM_UNSTABLE_MODULE_PATH}") + + # Add gtsam_unstable to the install target + list(APPEND GTSAM_PYTHON_DEPENDENCIES gtsam_unstable_py) -# Symlink all tests .py files to build folder. -create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" - "${GTSAM_UNSTABLE_MODULE_PATH}") endif() +# Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install - DEPENDS gtsam_py gtsam_unstable_py + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From 7477f9e0b08457bce087379d36f1e939e393924e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jan 2021 13:13:26 -0500 Subject: [PATCH 50/61] updated python setup files so that example data is loaded correctly --- python/MANIFEST.in | 1 + python/setup.py.in | 20 ++++++++------------ 2 files changed, 9 insertions(+), 12 deletions(-) create mode 100644 python/MANIFEST.in diff --git a/python/MANIFEST.in b/python/MANIFEST.in new file mode 100644 index 000000000..41d7f0c59 --- /dev/null +++ b/python/MANIFEST.in @@ -0,0 +1 @@ +recursive-include gtsam/Data * diff --git a/python/setup.py.in b/python/setup.py.in index 1ffe978f3..9aa4b71f4 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -1,6 +1,4 @@ -import glob -import os -import sys +"""Setup file to install the GTSAM package.""" try: from setuptools import setup, find_packages @@ -10,19 +8,17 @@ except ImportError: packages = find_packages(where=".") print("PACKAGES: ", packages) -data_path = '${GTSAM_SOURCE_DIR}/examples/Data/' -data_files_and_directories = glob.glob(data_path + '**', recursive=True) -data_files = [x for x in data_files_and_directories if not os.path.isdir(x)] - package_data = { '': [ - './*.so', - './*.dll', + "./*.so", + "./*.dll", + "Data/*" # Add the data files to the package + "Data/**/*" # Add the data files in subdirectories ] } # Cleaner to read in the contents rather than copy them over. -readme_contents = open("${PROJECT_SOURCE_DIR}/README.md").read() +readme_contents = open("${GTSAM_SOURCE_DIR}/README.md").read() setup( name='gtsam', @@ -49,9 +45,9 @@ setup( 'Programming Language :: Python :: 3', ], packages=packages, + include_package_data=True, package_data=package_data, - data_files=[('${GTSAM_PYTHON_DATASET_DIR}', data_files),], test_suite="gtsam.tests", - install_requires=["numpy"], + install_requires=open("${GTSAM_SOURCE_DIR}/python/requirements.txt").readlines(), zip_safe=False, ) From 8540b2c07d8852971ff05ca77295f4485e953ed7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jan 2021 13:13:44 -0500 Subject: [PATCH 51/61] more precise python version control in CI --- .github/scripts/python.sh | 5 +++-- .github/workflows/build-python.yml | 9 ++++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index a71e14c97..80f531229 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -43,7 +43,7 @@ if [ -z ${PYTHON_VERSION+x} ]; then exit 127 fi -PYTHON="python${PYTHON_VERSION}" +PYTHON="python${PYTHON_MAJOR_VERSION}" if [[ $(uname) == "Darwin" ]]; then brew install wget @@ -66,7 +66,8 @@ mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ - -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \ + -DGTSAM_BUILD_TESTS=OFF \ + -DGTSAM_BUILD_UNSTABLE=ON \ -DGTSAM_USE_QUATERNIONS=OFF \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 3f9a2e98a..e4eafe920 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -12,6 +12,8 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} + PYTHON_MAJOR_VERSION: 3 + strategy: fail-fast: false matrix: @@ -26,32 +28,37 @@ jobs: ] build_type: [Debug, Release] - python_version: [3] + include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 compiler: gcc version: "5" + python_version: 3.6.9 - name: ubuntu-18.04-gcc-9 os: ubuntu-18.04 compiler: gcc version: "9" + python_version: 3.6.9 - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 compiler: clang version: "9" + python_version: 3.6.9 - name: macOS-10.15-xcode-11.3.1 os: macOS-10.15 compiler: xcode version: "11.3.1" + python_version: 3.9.1 - name: ubuntu-18.04-gcc-5-tbb os: ubuntu-18.04 compiler: gcc version: "5" + python_version: 3.6.9 flag: tbb steps: From 569311d49cad895909d6460e7a86a74a117c6070 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jan 2021 13:26:34 -0500 Subject: [PATCH 52/61] Revert "more precise python version control in CI" This reverts commit 8540b2c07d8852971ff05ca77295f4485e953ed7. --- .github/scripts/python.sh | 5 ++--- .github/workflows/build-python.yml | 9 +-------- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 80f531229..a71e14c97 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -43,7 +43,7 @@ if [ -z ${PYTHON_VERSION+x} ]; then exit 127 fi -PYTHON="python${PYTHON_MAJOR_VERSION}" +PYTHON="python${PYTHON_VERSION}" if [[ $(uname) == "Darwin" ]]; then brew install wget @@ -66,8 +66,7 @@ mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ - -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_BUILD_UNSTABLE=ON \ + -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \ -DGTSAM_USE_QUATERNIONS=OFF \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index e4eafe920..3f9a2e98a 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -12,8 +12,6 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} - PYTHON_MAJOR_VERSION: 3 - strategy: fail-fast: false matrix: @@ -28,37 +26,32 @@ jobs: ] build_type: [Debug, Release] - + python_version: [3] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 compiler: gcc version: "5" - python_version: 3.6.9 - name: ubuntu-18.04-gcc-9 os: ubuntu-18.04 compiler: gcc version: "9" - python_version: 3.6.9 - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 compiler: clang version: "9" - python_version: 3.6.9 - name: macOS-10.15-xcode-11.3.1 os: macOS-10.15 compiler: xcode version: "11.3.1" - python_version: 3.9.1 - name: ubuntu-18.04-gcc-5-tbb os: ubuntu-18.04 compiler: gcc version: "5" - python_version: 3.6.9 flag: tbb steps: From 5df235d714618c0ce50e72a03422929be113c840 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 4 Jan 2021 20:45:22 -0500 Subject: [PATCH 53/61] add virtual constructor --- gtsam/geometry/Cal3.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 74c9868f3..08ce4c1e6 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -99,6 +99,9 @@ class GTSAM_EXPORT Cal3 { */ Cal3(double fov, int w, int h); + /// Virtual destructor + virtual ~Cal3() {} + /// @} /// @name Advanced Constructors /// @{ From 4d100461d4ca0d54fcee02bde8bb473ab3c6ded5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 4 Jan 2021 20:46:16 -0500 Subject: [PATCH 54/61] Removed reference for iterating over values. Also used auto where I could, when changing. --- examples/Pose3Localization.cpp | 4 ++-- examples/Pose3SLAMExample_changeKeys.cpp | 2 +- examples/Pose3SLAMExample_g2o.cpp | 2 +- examples/Pose3SLAMExample_initializePose3Chordal.cpp | 2 +- examples/Pose3SLAMExample_initializePose3Gradient.cpp | 2 +- examples/SolverComparer.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 2 +- gtsam/linear/VectorValues.cpp | 4 ++-- gtsam/nonlinear/NonlinearFactorGraph.cpp | 2 +- gtsam/nonlinear/Values-inl.h | 4 ++-- gtsam/nonlinear/Values.cpp | 4 ++-- gtsam/nonlinear/internal/LevenbergMarquardtState.h | 2 +- gtsam/nonlinear/tests/testValues.cpp | 6 +++--- gtsam/slam/InitializePose.h | 4 ++-- gtsam/slam/InitializePose3.cpp | 8 ++++---- gtsam/slam/dataset.cpp | 2 +- gtsam/slam/tests/testLago.cpp | 4 ++-- .../ConcurrentFilteringAndSmoothingExample.cpp | 4 ++-- gtsam_unstable/examples/SmartRangeExample_plaza1.cpp | 4 ++-- gtsam_unstable/examples/SmartRangeExample_plaza2.cpp | 4 ++-- gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp | 4 ++-- gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp | 10 +++++----- gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp | 10 +++++----- .../nonlinear/ConcurrentIncrementalFilter.cpp | 6 +++--- .../nonlinear/ConcurrentIncrementalSmoother.cpp | 8 ++++---- .../nonlinear/tests/testConcurrentBatchFilter.cpp | 2 +- .../nonlinear/tests/testConcurrentBatchSmoother.cpp | 2 +- .../tests/testConcurrentIncrementalFilter.cpp | 2 +- .../tests/testConcurrentIncrementalSmootherDL.cpp | 4 ++-- .../tests/testConcurrentIncrementalSmootherGN.cpp | 4 ++-- 30 files changed, 60 insertions(+), 60 deletions(-) diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index 1667b43d9..c18a9e9ce 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const Values::ConstKeyValuePair& key_value : *initial) { + for (const auto key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); @@ -74,7 +74,7 @@ int main(const int argc, const char* argv[]) { // Calculate and print marginal covariances for all variables Marginals marginals(*graph, result); - for (const auto& key_value : result) { + for (const auto key_value : result) { auto p = dynamic_cast*>(&key_value.value); if (!p) continue; std::cout << marginals.marginalCovariance(key_value.key) << endl; diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index abadce975..5d4ed6657 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) { std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; // Additional: rewrite input with simplified keys 0,1,... Values simpleInitial; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for(const auto key_value: *initial) { Key key; if(add) key = key_value.key + firstKey; diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 1cca92f59..367964307 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const Values::ConstKeyValuePair& key_value : *initial) { + for (const auto key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index 00a546bb2..992750fed 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const Values::ConstKeyValuePair& key_value : *initial) { + for (const auto key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 2f92afb9e..384f290a1 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const Values::ConstKeyValuePair& key_value : *initial) { + for (const auto key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 718ae6286..7bae41403 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -559,7 +559,7 @@ void runPerturb() // Perturb values VectorValues noise; - for(const Values::KeyValuePair& key_val: initial) + for(const Values::KeyValuePair key_val: initial) { Vector noisev(key_val.value.dim()); for(Vector::Index i = 0; i < noisev.size(); ++i) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 889f68580..290945837 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -825,7 +825,7 @@ TEST(Rot3, RQ_derivative) { const auto R = Rot3::RzRyRx(xyz).matrix(); const auto num = numericalDerivative11(RQ_proxy, R); Matrix39 calc; - RQ(R, calc).second; + auto dummy = RQ(R, calc).second; const auto err = vec_err.second; CHECK(assert_equal(num, calc, err)); diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 9b5868744..746275847 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -161,7 +161,7 @@ namespace gtsam { bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; - for(const auto& values: boost::combine(*this, x)) { + for(const auto values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -233,7 +233,7 @@ namespace gtsam { double result = 0.0; typedef boost::tuple ValuePair; using boost::adaptors::map_values; - for(const ValuePair& values: boost::combine(*this, v)) { + for(const ValuePair values: boost::combine(*this, v)) { assert_throw(values.get<0>().first == values.get<1>().first, invalid_argument("VectorValues::dot called with a VectorValues of different structure")); assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 3063aa329..6a9e0cd0a 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -376,7 +376,7 @@ static Scatter scatterFromValues(const Values& values) { scatter.reserve(values.size()); // use "natural" ordering with keys taken from the initial values - for (const auto& key_value : values) { + for (const auto key_value : values) { scatter.add(key_value.key, key_value.value.dim()); } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 6829e859b..19813adba 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -217,7 +217,7 @@ namespace gtsam { /** Constructor from a Filtered view copies out all values */ template Values::Values(const Values::Filtered& view) { - for(const typename Filtered::KeyValuePair& key_value: view) { + for(const auto key_value: view) { Key key = key_value.key; insert(key, static_cast(key_value.value)); } @@ -226,7 +226,7 @@ namespace gtsam { /* ************************************************************************* */ template Values::Values(const Values::ConstFiltered& view) { - for(const typename ConstFiltered::KeyValuePair& key_value: view) { + for(const auto key_value: view) { Key key = key_value.key; insert(key, static_cast(key_value.value)); } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b672031ca..89a4206ee 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -206,7 +206,7 @@ namespace gtsam { /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; - for(const ConstKeyValuePair& key_value: *this) { + for(const auto key_value: *this) { result += key_value.value.dim(); } return result; @@ -215,7 +215,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues Values::zeroVectors() const { VectorValues result; - for(const ConstKeyValuePair& key_value: *this) + for(const auto key_value: *this) result.insert(key_value.key, Vector::Zero(key_value.value.dim())); return result; } diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index 8ab2e7466..cee839540 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -126,7 +126,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { noiseModelCache.resize(0); // for each of the variables, add a prior damped.reserve(damped.size() + values.size()); - for (const auto& key_value : values) { + for (const auto key_value : values) { const Key key = key_value.key; const size_t dim = key_value.value.dim(); const CachedModel* item = getCachedModel(dim); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 88cfb666f..a095cc381 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -335,7 +335,7 @@ TEST(Values, filter) { int i = 0; Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); EXPECT_LONGS_EQUAL(2, (long)filtered.size()); - for(const Values::Filtered<>::KeyValuePair& key_value: filtered) { + for(const auto key_value: filtered) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} @@ -370,7 +370,7 @@ TEST(Values, filter) { i = 0; Values::ConstFiltered pose_filtered = values.filter(); EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size()); - for(const Values::ConstFiltered::KeyValuePair& key_value: pose_filtered) { + for(const auto key_value: pose_filtered) { if(i == 0) { EXPECT_LONGS_EQUAL(1, (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value)); @@ -408,7 +408,7 @@ TEST(Values, Symbol_filter) { values.insert(Symbol('y', 3), pose3); int i = 0; - for(const Values::Filtered::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) { + for(const auto key_value: values.filter(Symbol::ChrTest('y'))) { if(i == 0) { LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value.cast())); diff --git a/gtsam/slam/InitializePose.h b/gtsam/slam/InitializePose.h index be249b0b5..f9b99e47e 100644 --- a/gtsam/slam/InitializePose.h +++ b/gtsam/slam/InitializePose.h @@ -62,7 +62,7 @@ static Values computePoses(const Values& initialRot, // Upgrade rotations to full poses Values initialPose; - for (const auto& key_value : initialRot) { + for (const auto key_value : initialRot) { Key key = key_value.key; const auto& rot = initialRot.at(key); Pose initializedPose = Pose(rot, origin); @@ -86,7 +86,7 @@ static Values computePoses(const Values& initialRot, // put into Values structure Values estimate; - for (const auto& key_value : GNresult) { + for (const auto key_value : GNresult) { Key key = key_value.key; if (key != kAnchorKey) { const Pose& pose = GNresult.at(key); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index af1fc609e..43404df53 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -124,7 +124,7 @@ Values InitializePose3::computeOrientationsGradient( // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; inverseRot.insert(initialize::kAnchorKey, Rot3()); - for(const auto& key_value: givenGuess) { + for(const auto key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -139,7 +139,7 @@ Values InitializePose3::computeOrientationsGradient( // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - for(const auto& key_value: inverseRot) { + for(const auto key_value: inverseRot) { Key key = key_value.key; grad.insert(key,Z_3x1); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); @@ -162,7 +162,7 @@ Values InitializePose3::computeOrientationsGradient( ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node maxGrad = 0; - for (const auto& key_value : inverseRot) { + for (const auto key_value : inverseRot) { Key key = key_value.key; Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key @@ -203,7 +203,7 @@ Values InitializePose3::computeOrientationsGradient( // Return correct rotations const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const auto& key_value: inverseRot) { + for(const auto key_value: inverseRot) { Key key = key_value.key; if (key != initialize::kAnchorKey) { const Rot3& R = inverseRot.at(key); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 74e074c9e..c8a8b15c5 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -586,7 +586,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, fstream stream(filename.c_str(), fstream::out); // save poses - for (const Values::ConstKeyValuePair key_value : config) { + for (const auto key_value : config) { const Pose2 &pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 55449d86e..781317d7a 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -284,7 +284,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - for(const Values::KeyValuePair& key_val: *expected){ + for(const auto key_val: *expected){ Key k = key_val.key; EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); } @@ -310,7 +310,7 @@ TEST( Lago, largeGraphNoisy ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - for(const Values::KeyValuePair& key_val: *expected){ + for(const auto key_val: *expected){ Key k = key_val.key; EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); } diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 939d3a5c8..1494d784b 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -308,11 +308,11 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << " Concurrent Filter Keys: " << endl; - for(const auto& key_value: concurrentFilter.getLinearizationPoint()) { + for(const auto key_value: concurrentFilter.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; } cout << " Concurrent Smoother Keys: " << endl; - for(const auto& key_value: concurrentSmoother.getLinearizationPoint()) { + for(const auto key_value: concurrentSmoother.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; } cout << " Fixed-Lag Smoother Keys: " << endl; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 5fdc7a743..6a1239fba 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -233,7 +233,7 @@ int main(int argc, char** argv) { } } countK = 0; - for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) + for(const auto it: result.filter()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; if (smart) { @@ -256,7 +256,7 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os("rangeResult.txt"); - for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) + for(const auto it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; exit(0); diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 90b2a30ff..95aff85a7 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -202,11 +202,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os2("rangeResultLM.txt"); - for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) + for(const auto it: result.filter()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; ofstream os("rangeResult.txt"); - for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) + for(const auto it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; exit(0); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 777e4b2d3..fd18e7c6d 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -59,7 +59,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - for (const auto& key_value : newTheta) { + for (const auto key_value : newTheta) { ordering_.push_back(key_value.key); } // Augment Delta @@ -267,7 +267,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Put the linearization points and deltas back for specific variables if (enforceConsistency_ && (linearKeys_.size() > 0)) { theta_.update(linearKeys_); - for(const auto& key_value: linearKeys_) { + for(const auto key_value: linearKeys_) { delta_.at(key_value.key) = newDelta.at(key_value.key); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 758bcfe48..83d0ab719 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -139,7 +139,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - for(const Values::ConstKeyValuePair& key_value: newTheta) { + for(const auto key_value: newTheta) { ordering_.push_back(key_value.key); } // Augment Delta @@ -222,7 +222,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm // Find the set of new separator keys KeySet newSeparatorKeys; - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -236,7 +236,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm graph.push_back(smootherShortcut_); Values values; values.insert(smootherSummarizationValues); - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { if(!values.exists(key_value.key)) { values.insert(key_value.key, key_value.value); } @@ -471,7 +471,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values // Put the linearization points and deltas back for specific variables if(linearValues.size() > 0) { theta.update(linearValues); - for(const Values::ConstKeyValuePair& key_value: linearValues) { + for(const auto key_value: linearValues) { delta.at(key_value.key) = newDelta.at(key_value.key); } } @@ -574,7 +574,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove KeySet newSeparatorKeys = removedFactors.keys(); - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { newSeparatorKeys.insert(key_value.key); } for(Key key: keysToMove) { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 600baa9f0..75d491bde 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -61,7 +61,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF theta_.insert(newTheta); // Add new variables to the end of the ordering - for(const Values::ConstKeyValuePair& key_value: newTheta) { + for(const auto key_value: newTheta) { ordering_.push_back(key_value.key); } @@ -135,7 +135,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa removeFactors(filterSummarizationSlots_); // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta - for(const Values::ConstKeyValuePair& key_value: smootherValues) { + for(const auto key_value: smootherValues) { std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); if(iter_inserted.second) { // If the insert succeeded @@ -146,7 +146,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa iter_inserted.first->value = key_value.value; } } - for(const Values::ConstKeyValuePair& key_value: separatorValues) { + for(const auto key_value: separatorValues) { std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); if(iter_inserted.second) { // If the insert succeeded @@ -322,7 +322,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { // Put the linearization points and deltas back for specific variables if(separatorValues_.size() > 0) { theta_.update(separatorValues_); - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { delta_.at(key_value.key) = newDelta.at(key_value.key); } } @@ -372,7 +372,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { // Get the set of separator keys gtsam::KeySet separatorKeys; - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index b70b9efc2..b9cf66a97 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -69,13 +69,13 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No int group = 1; // Set all existing variables to Group1 if(isam2_.getLinearizationPoint().size() > 0) { - for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) { + for(const auto key_value: isam2_.getLinearizationPoint()) { orderingConstraints->operator[](key_value.key) = group; } ++group; } // Assign new variables to the root - for(const Values::ConstKeyValuePair& key_value: newTheta) { + for(const auto key_value: newTheta) { orderingConstraints->operator[](key_value.key) = group; } // Set marginalizable variables to Group0 @@ -201,7 +201,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth // Force iSAM2 not to relinearize anything during this iteration FastList noRelinKeys; - for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) { + for(const auto key_value: isam2_.getLinearizationPoint()) { noRelinKeys.push_back(key_value.key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 714d7c8d0..3886d0e42 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -66,7 +66,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons // Also, mark the separator keys as fixed linearization points FastMap constrainedKeys; FastList noRelinKeys; - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { constrainedKeys[key_value.key] = 1; noRelinKeys.push_back(key_value.key); } @@ -82,12 +82,12 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons Values values(newTheta); // Unfortunately, we must be careful here, as some of the smoother values // and/or separator values may have been added previously - for(const Values::ConstKeyValuePair& key_value: smootherValues_) { + for(const auto key_value: smootherValues_) { if(!isam2_.getLinearizationPoint().exists(key_value.key)) { values.insert(key_value.key, smootherValues_.at(key_value.key)); } } - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { if(!isam2_.getLinearizationPoint().exists(key_value.key)) { values.insert(key_value.key, separatorValues_.at(key_value.key)); } @@ -246,7 +246,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { // Get the set of separator keys KeySet separatorKeys; - for(const Values::ConstKeyValuePair& key_value: separatorValues_) { + for(const auto key_value: separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 1f19c0e8a..61db05167 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -64,7 +64,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, std::set KeysToKeep; - for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph + for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph KeysToKeep.insert(key_value.key); } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize for(Key key: keysToMarginalize) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index ae9a3f827..b5fb61428 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -560,7 +560,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); KeySet eliminateKeys = linearFactors->keys(); - for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { + for(const auto key_value: filterSeparatorValues) { eliminateKeys.erase(key_value.key); } KeyVector variables(eliminateKeys.begin(), eliminateKeys.end()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index c5dba1a69..08d71a420 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -80,7 +80,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, std::set KeysToKeep; - for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph + for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph KeysToKeep.insert(key_value.key); } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize for(Key key: keysToMarginalize) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 5de115013..ccb5a104e 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -512,7 +512,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 ) // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); Values expectedLinearizationPoint = filterSeparatorValues; Values actualLinearizationPoint; - for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { + for(const auto key_value: filterSeparatorValues) { actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); } CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); @@ -580,7 +580,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); KeySet allkeys = LinFactorGraph->keys(); - for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { + for(const auto key_value: filterSeparatorValues) { allkeys.erase(key_value.key); } KeyVector variables(allkeys.begin(), allkeys.end()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index ec78ee6e2..53c3d1610 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 ) // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); Values expectedLinearizationPoint = filterSeparatorValues; Values actualLinearizationPoint; - for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { + for(const auto key_value: filterSeparatorValues) { actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); } CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); @@ -582,7 +582,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); KeySet allkeys = LinFactorGraph->keys(); - for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) + for(const auto key_value: filterSeparatorValues) allkeys.erase(key_value.key); KeyVector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); From 02abc53fc15ebb45a3a468293b737e8352c2b549 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 10:07:06 -0500 Subject: [PATCH 55/61] fix metis based warnings in CMake and compiling --- gtsam/3rdparty/metis/CMakeLists.txt | 2 +- gtsam/3rdparty/metis/libmetis/CMakeLists.txt | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index de46165ff..dc26aecb2 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.0) project(METIS) # Add flags for currect directory and below diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index 330e989fa..92f931b98 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -12,6 +12,7 @@ endif() if(WIN32) set_target_properties(metis-gtsam PROPERTIES PREFIX "" + COMPILE_FLAGS /w RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") endif() From a650a6f8b1fb68da84b2fab17fedfbbe26d407cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 10:43:31 -0500 Subject: [PATCH 56/61] add std namespacing --- gtsam/geometry/Pose3.cpp | 4 ++-- gtsam/geometry/SOn-inl.h | 4 +--- gtsam/sfm/ShonanAveraging.h | 4 ++-- gtsam/slam/dataset.h | 8 ++++---- 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 22849d4f5..c183e32ed 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -106,8 +106,8 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, } /* ************************************************************************* */ -void Pose3::print(const string& s) const { - cout << (s.empty() ? s : s + " ") << *this << endl; +void Pose3::print(const std::string& s) const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 6180f4cc7..284ae76de 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -22,8 +22,6 @@ #include -using namespace std; - namespace gtsam { // Implementation for N>=5 just uses dynamic version @@ -108,7 +106,7 @@ typename SO::VectorN2 SO::vec( template void SO::print(const std::string& s) const { - cout << s << matrix_ << endl; + std::cout << s << matrix_ << std::endl; } } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index edd9f33a2..f1ce31fa6 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -354,7 +354,7 @@ class ShonanAveraging2 : public ShonanAveraging<2> { public: ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters = Parameters()); - explicit ShonanAveraging2(string g2oFile, + explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); }; @@ -362,7 +362,7 @@ class ShonanAveraging3 : public ShonanAveraging<3> { public: ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters = Parameters()); - explicit ShonanAveraging3(string g2oFile, + explicit ShonanAveraging3(std::string g2oFile, const Parameters ¶meters = Parameters()); // TODO(frank): Deprecate after we land pybind wrapper diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d96c11167..80877c136 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -303,8 +303,8 @@ struct SfmTrack { /// print void print(const std::string& s = "") const { - cout << "Track with " << measurements.size(); - cout << " measurements of point " << p << "\n"; + std::cout << "Track with " << measurements.size(); + std::cout << " measurements of point " << p << std::endl; } }; @@ -385,8 +385,8 @@ struct SfmData { /// print void print(const std::string& s = "") const { - cout << "Number of cameras = " << number_cameras() << "\n"; - cout << "Number of tracks = " << number_tracks() << "\n"; + std::cout << "Number of cameras = " << number_cameras() << std::endl; + std::cout << "Number of tracks = " << number_tracks() << std::endl; } }; From 473a6a15ee67bbdf559bb58932773cd8183e1f00 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 10:44:05 -0500 Subject: [PATCH 57/61] fix warnings for vectors and matrices --- gtsam/nonlinear/tests/testValues.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 88cfb666f..455af942c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -175,10 +175,13 @@ TEST(Values, basic_functions) { Values values; const Values& values_c = values; - values.insert(2, Vector3()); - values.insert(4, Vector3()); - values.insert(6, Matrix23()); - values.insert(8, Matrix23()); + Matrix23 M1, M2; + M1 << 0, 0, 0, 0, 0, 0; + M2 << 0, 0, 0, 0, 0, 0; + values.insert(2, Vector3(0, 0, 0)); + values.insert(4, Vector3(0, 0, 0)); + values.insert(6, M1); + values.insert(8, M2); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); From b7584ce362fd30fe498292f6c32754983a0cc34f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 10:44:26 -0500 Subject: [PATCH 58/61] verbose printing of exceptions --- gtsam/slam/GeneralSFMFactor.h | 24 +++++++++++++------ .../examples/SmartRangeExample_plaza1.cpp | 3 ++- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index f848a56ca..3e292c892 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -70,6 +70,7 @@ class GeneralSFMFactor: public NoiseModelFactor2 { protected: Point2 measured_; ///< the 2D measurement + bool verbose_; ///< Flag for print verbosity public: @@ -86,12 +87,17 @@ public: * @param cameraKey is the index of the camera * @param landmarkKey is the index of the landmark */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : - Base(model, cameraKey, landmarkKey), measured_(measured) {} + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, + Key cameraKey, Key landmarkKey, bool verbose = false) + : Base(model, cameraKey, landmarkKey), + measured_(measured), + verbose_(verbose) {} - GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor - GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 - GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 + GeneralSFMFactor() : measured_(0.0, 0.0) {} ///< default constructor + ///< constructor that takes a Point2 + GeneralSFMFactor(const Point2& p) : measured_(p) {} + ///< constructor that takes doubles x,y to make a Point2 + GeneralSFMFactor(double x, double y) : measured_(x, y) {} virtual ~GeneralSFMFactor() {} ///< destructor @@ -127,7 +133,9 @@ public: catch( CheiralityException& e) { if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); - // TODO warn if verbose output asked for + if (verbose_) { + std::cout << e.what() << std::endl; + } return Z_2x1; } } @@ -149,7 +157,9 @@ public: H1.setZero(); H2.setZero(); b.setZero(); - // TODO warn if verbose output asked for + if (verbose_) { + std::cout << e.what() << std::endl; + } } // Whiten the system if needed diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 5fdc7a743..0976674de 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -188,7 +188,8 @@ int main(int argc, char** argv) { smartFactors[j]->addRange(i, range); printf("adding range %g for %d",range,(int)j); } catch (const invalid_argument& e) { - printf("warning: omitting duplicate range %g for %d",range,(int)j); + printf("warning: omitting duplicate range %g for %d: %s", range, + (int)j, e.what()); } cout << endl; } From b244a7d6366c0d3a29080b2ea3fc00306ec1ab8d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 13:59:58 -0500 Subject: [PATCH 59/61] remove verbose flag and print exception to std::cerr --- gtsam/slam/GeneralSFMFactor.h | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 3e292c892..1e48571e3 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -70,7 +70,6 @@ class GeneralSFMFactor: public NoiseModelFactor2 { protected: Point2 measured_; ///< the 2D measurement - bool verbose_; ///< Flag for print verbosity public: @@ -88,10 +87,8 @@ public: * @param landmarkKey is the index of the landmark */ GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, - Key cameraKey, Key landmarkKey, bool verbose = false) - : Base(model, cameraKey, landmarkKey), - measured_(measured), - verbose_(verbose) {} + Key cameraKey, Key landmarkKey) + : Base(model, cameraKey, landmarkKey), measured_(measured) {} GeneralSFMFactor() : measured_(0.0, 0.0) {} ///< default constructor ///< constructor that takes a Point2 @@ -133,9 +130,7 @@ public: catch( CheiralityException& e) { if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); - if (verbose_) { - std::cout << e.what() << std::endl; - } + std::cerr << e.what() << std::endl; return Z_2x1; } } @@ -157,9 +152,7 @@ public: H1.setZero(); H2.setZero(); b.setZero(); - if (verbose_) { - std::cout << e.what() << std::endl; - } + std::cerr << e.what() << std::endl; } // Whiten the system if needed From 5b52e4c29f6572e8e6d7a4c3abad95340e2668f4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 14:13:15 -0500 Subject: [PATCH 60/61] cleanly initialize matrices in test --- gtsam/nonlinear/tests/testValues.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 455af942c..bb66cb695 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -175,9 +175,7 @@ TEST(Values, basic_functions) { Values values; const Values& values_c = values; - Matrix23 M1, M2; - M1 << 0, 0, 0, 0, 0, 0; - M2 << 0, 0, 0, 0, 0, 0; + Matrix23 M1 = Matrix23::Zero(), M2 = Matrix23::Zero(); values.insert(2, Vector3(0, 0, 0)); values.insert(4, Vector3(0, 0, 0)); values.insert(6, M1); From 0f03ee1f7635f186cd6082dbc36b146b7d56fba7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Jan 2021 14:43:43 -0500 Subject: [PATCH 61/61] remove exception print, add TODO --- gtsam/slam/GeneralSFMFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 1e48571e3..c9639d4d5 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -130,7 +130,7 @@ public: catch( CheiralityException& e) { if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); - std::cerr << e.what() << std::endl; + //TODO Print the exception via logging return Z_2x1; } } @@ -152,7 +152,7 @@ public: H1.setZero(); H2.setZero(); b.setZero(); - std::cerr << e.what() << std::endl; + //TODO Print the exception via logging } // Whiten the system if needed