From c4644a0d61e76f1b895d8c3c8fe7a70be4e8d55d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 22:50:41 -0500 Subject: [PATCH] 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;