From c163e28c311eccfff8a280338beda1bd2f058a82 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 8 May 2021 17:03:04 -0400 Subject: [PATCH 1/8] addeed gnc example --- .../examples/GncPoseAveragingExample.cpp | 91 +++++++++++++++++++ 1 file changed, 91 insertions(+) create mode 100644 gtsam_unstable/examples/GncPoseAveragingExample.cpp diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp new file mode 100644 index 0000000000..a5953bcfbe --- /dev/null +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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 GncPoseAveragingExample.cpp + * @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers + * @date May 8, 2021 + * @author Luca Carlone + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + cout << "== Robust Pose Averaging Example === " << endl; + + // default number of inliers and outliers + size_t nrInliers = 10; + size_t nrOutliers = 10; + + // User can pass arbitrary number of inliers and outliers for testing + if (argc > 1) + nrInliers = atoi(argv[1]); + if (argc > 2) + nrOutliers = atoi(argv[2]); + cout << "nrInliers " << nrInliers << " nrOutliers "<< nrOutliers << endl; + + // Seed random number generator + random_device rd; + mt19937 rng(rd()); + uniform_real_distribution uniformOutliers(-10, 10); + normal_distribution normalInliers(0.0, 0.05); + + Values initial; + initial.insert(0, Pose3::identity()); // identity pose as initialization + + // create ground truth pose + Pose3 gtPose = Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) ); + + NonlinearFactorGraph graph; + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,0.05); + // create inliers + for(size_t i=0; i(0,poseMeasurement,model)); + } + + // create outliers + for(size_t i=0; i(0,poseMeasurement,model)); + } + + GncParams gncParams; + auto gnc = GncOptimizer>(graph, + initial, + gncParams); + + Values estimate = gnc.optimize(); + Pose3 poseError = gtPose.between( estimate.at(0) ); + cout << "norm of translation error: " << poseError.translation().norm() << + " norm of rotation error: " << poseError.rotation().rpy().norm() << endl; + // poseError.print("pose error: \n "); + return 0; +} From 7342438fb3af58efa2cf2f0b41a1a0f5d5fa51dc Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 10 May 2021 10:30:17 -0400 Subject: [PATCH 2/8] added GNC example --- gtsam_unstable/examples/GncPoseAveragingExample.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp index a5953bcfbe..7af577e18d 100644 --- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -46,14 +46,18 @@ int main(int argc, char** argv){ // Seed random number generator random_device rd; mt19937 rng(rd()); - uniform_real_distribution uniformOutliers(-10, 10); + uniform_real_distribution uniform(-10, 10); normal_distribution normalInliers(0.0, 0.05); Values initial; initial.insert(0, Pose3::identity()); // identity pose as initialization // create ground truth pose - Pose3 gtPose = Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) ); + Vector6 poseGtVector; + for(size_t i = 0; i < 6; ++i){ + poseGtVector(i) = uniform(rng); + } + Pose3 gtPose = Pose3::Expmap(poseGtVector); // Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) ); NonlinearFactorGraph graph; const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,0.05); @@ -71,7 +75,7 @@ int main(int argc, char** argv){ for(size_t i=0; i(0,poseMeasurement,model)); From 3ac97c3dbed05b458b3a5d36760796ba6cc28f57 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 10 May 2021 10:30:32 -0400 Subject: [PATCH 3/8] adding knownOutlier input to GNC --- gtsam/nonlinear/GncOptimizer.h | 52 ++++++++++++++++++++++-- gtsam/nonlinear/GncParams.h | 21 +++++++++- tests/testGncOptimizer.cpp | 73 ++++++++++++++++++++++++++++++++++ 3 files changed, 140 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index e1efe7132a..6683964394 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -74,6 +74,32 @@ class GncOptimizer { } } + // check that known inliers and outliers make sense: + std::vector incorrectlySpecifiedWeights; // measurements the user has incorrectly specified + // to be BOTH known inliers and known outliers + std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(), + params.knownOutliers.begin(), + params.knownOutliers.end(), + std::inserter(incorrectlySpecifiedWeights, incorrectlySpecifiedWeights.begin())); + if(incorrectlySpecifiedWeights.size() > 0){ + params.print("params\n"); + throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements" + " to be both a known inlier and a known outlier."); + } + // check that known inliers are in the graph + for (size_t i = 0; i < params.knownInliers.size(); i++){ + if( params.knownInliers[i] > nfg_.size()-1 ){ + throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements" + "that are not in the factor graph to be known inliers."); + } + } + // check that known outliers are in the graph + for (size_t i = 0; i < params.knownOutliers.size(); i++){ + if( params.knownOutliers[i] > nfg_.size()-1 ){ + throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements" + "that are not in the factor graph to be known outliers."); + } + } // set default barcSq_ (inlier threshold) double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_ setInlierCostThresholdsAtProbability(alpha); @@ -132,10 +158,18 @@ class GncOptimizer { && equal(barcSq_, other.getInlierCostThresholds()); } + Vector initializeWeightsFromKnownInliersAndOutliers() const{ + Vector weights = Vector::Ones(nfg_.size()); + for (size_t i = 0; i < params_.knownOutliers.size(); i++){ + weights[ params_.knownOutliers[i] ] = 0.0; + } + return weights; + } + /// Compute optimal solution using graduated non-convexity. Values optimize() { // start by assuming all measurements are inliers - weights_ = Vector::Ones(nfg_.size()); + weights_ = initializeWeightsFromKnownInliersAndOutliers(); BaseOptimizer baseOptimizer(nfg_, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); @@ -146,12 +180,18 @@ class GncOptimizer { // 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_.verbosity >= GncParameters::Verbosity::SUMMARY) { + int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() ); + // ^^ number of measurements that are not known to be inliers or outliers + if (mu <= 0 || nrUnknownInOrOut == 0) { + if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " "initialization is small." << std::endl; } + if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) { + std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers" + << std::endl; + } if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); std::cout << "mu: " << mu << std::endl; @@ -350,7 +390,7 @@ class GncOptimizer { /// Calculate gnc weights. Vector calculateWeights(const Values& currentEstimate, const double mu) { - Vector weights = Vector::Ones(nfg_.size()); + Vector weights = initializeWeightsFromKnownInliersAndOutliers(); // do not update the weights that the user has decided are known inliers std::vector allWeights; @@ -362,6 +402,10 @@ class GncOptimizer { params_.knownInliers.begin(), params_.knownInliers.end(), std::inserter(unknownWeights, unknownWeights.begin())); + std::set_difference(unknownWeights.begin(), unknownWeights.end(), + params_.knownOutliers.begin(), + params_.knownOutliers.end(), + std::inserter(unknownWeights, unknownWeights.begin())); // update weights of known inlier/outlier measurements switch (params_.lossType) { diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 904d7b4349..c1bf7a0352 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -71,6 +71,7 @@ class GncParams { 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 + std::vector knownOutliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -112,8 +113,21 @@ class GncParams { * 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++) + for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); + } + std::sort(knownInliers.begin(), knownInliers.end()); + } + + /** (Optional) Provide a vector of measurements that must be considered outliers. 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 knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. + * */ + void setKnownOutliers(const std::vector& knownOut) { + for (size_t i = 0; i < knownOut.size(); i++){ + knownOutliers.push_back(knownOut[i]); + } + std::sort(knownOutliers.begin(), knownOutliers.end()); } /// Equals. @@ -121,7 +135,8 @@ class GncParams { return baseOptimizerParams.equals(other.baseOptimizerParams) && lossType == other.lossType && maxIterations == other.maxIterations && std::fabs(muStep - other.muStep) <= tol - && verbosity == other.verbosity && knownInliers == other.knownInliers; + && verbosity == other.verbosity && knownInliers == other.knownInliers + && knownOutliers == other.knownOutliers; } /// Print. @@ -144,6 +159,8 @@ class GncParams { std::cout << "verbosity: " << verbosity << "\n"; for (size_t i = 0; i < knownInliers.size(); i++) std::cout << "knownInliers: " << knownInliers[i] << "\n"; + for (size_t i = 0; i < knownOutliers.size(); i++) + std::cout << "knownOutliers: " << knownOutliers[i] << "\n"; baseOptimizerParams.print(str); } }; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 981e475ca1..175263bce4 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -749,6 +749,79 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! } +/* ************************************************************************* */ +TEST(GncOptimizer, knownInliersAndOutliers) { + 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); + + fg.print(" \n "); + +// // nonconvexity with known inliers +// { +// GncParams gncParams; +// gncParams.setKnownInliers(knownInliers); +// gncParams.setLossType(GncLossType::GM); +// //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); +// auto gnc = GncOptimizer>(fg, initial, +// gncParams); +// gnc.setInlierCostThresholds(1.0); +// Values gnc_result = gnc.optimize(); +// CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); +// +// // check weights were actually fixed: +// Vector finalWeights = gnc.getWeights(); +// DOUBLES_EQUAL(1.0, finalWeights[0], tol); +// DOUBLES_EQUAL(1.0, finalWeights[1], tol); +// DOUBLES_EQUAL(1.0, finalWeights[2], tol); +// } +// { +// GncParams gncParams; +// gncParams.setKnownInliers(knownInliers); +// gncParams.setLossType(GncLossType::TLS); +// // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); +// auto gnc = GncOptimizer>(fg, initial, +// gncParams); +// +// Values gnc_result = gnc.optimize(); +// CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); +// +// // check weights were actually fixed: +// Vector finalWeights = gnc.getWeights(); +// DOUBLES_EQUAL(1.0, finalWeights[0], tol); +// DOUBLES_EQUAL(1.0, finalWeights[1], tol); +// DOUBLES_EQUAL(1.0, finalWeights[2], tol); +// DOUBLES_EQUAL(0.0, finalWeights[3], tol); +// } +// { +// // if we set the threshold large, they are all inliers +// GncParams gncParams; +// gncParams.setKnownInliers(knownInliers); +// gncParams.setLossType(GncLossType::TLS); +// //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); +// auto gnc = GncOptimizer>(fg, initial, +// gncParams); +// gnc.setInlierCostThresholds(100.0); +// +// 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); +// } +} + /* ************************************************************************* */ int main() { TestResult tr; From 5274abafd013280e9b5a29e2841b9b921cb17fc4 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 10 May 2021 18:23:12 -0400 Subject: [PATCH 4/8] all tests done! --- gtsam/nonlinear/GncOptimizer.h | 40 +++++----- tests/testGncOptimizer.cpp | 141 ++++++++++++++++++--------------- 2 files changed, 99 insertions(+), 82 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 6683964394..1a269468b5 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -75,27 +75,26 @@ class GncOptimizer { } // check that known inliers and outliers make sense: - std::vector incorrectlySpecifiedWeights; // measurements the user has incorrectly specified + std::vector inconsistentlySpecifiedWeights; // measurements the user has incorrectly specified // to be BOTH known inliers and known outliers std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(), - params.knownOutliers.begin(), - params.knownOutliers.end(), - std::inserter(incorrectlySpecifiedWeights, incorrectlySpecifiedWeights.begin())); - if(incorrectlySpecifiedWeights.size() > 0){ + params.knownOutliers.begin(),params.knownOutliers.end(), + std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin())); + if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently specified weights, we throw an exception params.print("params\n"); throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements" - " to be both a known inlier and a known outlier."); + " to be BOTH a known inlier and a known outlier."); } // check that known inliers are in the graph for (size_t i = 0; i < params.knownInliers.size(); i++){ - if( params.knownInliers[i] > nfg_.size()-1 ){ + if( params.knownInliers[i] > nfg_.size()-1 ){ // outside graph throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements" "that are not in the factor graph to be known inliers."); } } // check that known outliers are in the graph for (size_t i = 0; i < params.knownOutliers.size(); i++){ - if( params.knownOutliers[i] > nfg_.size()-1 ){ + if( params.knownOutliers[i] > nfg_.size()-1 ){ // outside graph throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements" "that are not in the factor graph to be known outliers."); } @@ -161,7 +160,7 @@ class GncOptimizer { Vector initializeWeightsFromKnownInliersAndOutliers() const{ Vector weights = Vector::Ones(nfg_.size()); for (size_t i = 0; i < params_.knownOutliers.size(); i++){ - weights[ params_.knownOutliers[i] ] = 0.0; + weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers } return weights; } @@ -170,10 +169,11 @@ class GncOptimizer { Values optimize() { // start by assuming all measurements are inliers weights_ = initializeWeightsFromKnownInliersAndOutliers(); - BaseOptimizer baseOptimizer(nfg_, state_); + NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_); + BaseOptimizer baseOptimizer(graph_initial, state_); Values result = baseOptimizer.optimize(); double mu = initializeMu(); - double prev_cost = nfg_.error(result); + double prev_cost = graph_initial.error(result); double cost = 0.0; // this will be updated in the main loop // handle the degenerate case that corresponds to small @@ -181,8 +181,8 @@ class GncOptimizer { // For GM: if residual error is small, mu -> 0 // For TLS: if residual error is small, mu -> -1 int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() ); - // ^^ number of measurements that are not known to be inliers or outliers - if (mu <= 0 || nrUnknownInOrOut == 0) { + // ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out) + if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "GNC Optimizer stopped because maximum residual at " "initialization is small." @@ -397,21 +397,21 @@ class GncOptimizer { for (size_t k = 0; k < nfg_.size(); k++) { allWeights.push_back(k); } + std::vector knownWeights; + std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(), + params_.knownOutliers.begin(), params_.knownOutliers.end(), + std::inserter(knownWeights, knownWeights.begin())); + std::vector unknownWeights; std::set_difference(allWeights.begin(), allWeights.end(), - params_.knownInliers.begin(), - params_.knownInliers.end(), + knownWeights.begin(), knownWeights.end(), std::inserter(unknownWeights, unknownWeights.begin())); - std::set_difference(unknownWeights.begin(), unknownWeights.end(), - params_.knownOutliers.begin(), - params_.knownOutliers.end(), - std::inserter(unknownWeights, unknownWeights.begin())); // update weights of known inlier/outlier measurements switch (params_.lossType) { case GncLossType::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 * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2); diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 175263bce4..7160c32fd5 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -757,69 +757,86 @@ TEST(GncOptimizer, knownInliersAndOutliers) { 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 and known outliers (check early stopping + // when all measurements are known to be inliers or outliers) + { + std::vector knownInliers; + knownInliers.push_back(0); + knownInliers.push_back(1); + knownInliers.push_back(2); + + std::vector knownOutliers; + knownOutliers.push_back(3); + + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setKnownOutliers(knownOutliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setInlierCostThresholds(1.0); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); - fg.print(" \n "); - -// // nonconvexity with known inliers -// { -// GncParams gncParams; -// gncParams.setKnownInliers(knownInliers); -// gncParams.setLossType(GncLossType::GM); -// //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); -// auto gnc = GncOptimizer>(fg, initial, -// gncParams); -// gnc.setInlierCostThresholds(1.0); -// Values gnc_result = gnc.optimize(); -// CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); -// -// // check weights were actually fixed: -// Vector finalWeights = gnc.getWeights(); -// DOUBLES_EQUAL(1.0, finalWeights[0], tol); -// DOUBLES_EQUAL(1.0, finalWeights[1], tol); -// DOUBLES_EQUAL(1.0, finalWeights[2], tol); -// } -// { -// GncParams gncParams; -// gncParams.setKnownInliers(knownInliers); -// gncParams.setLossType(GncLossType::TLS); -// // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); -// auto gnc = GncOptimizer>(fg, initial, -// gncParams); -// -// Values gnc_result = gnc.optimize(); -// CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); -// -// // check weights were actually fixed: -// Vector finalWeights = gnc.getWeights(); -// DOUBLES_EQUAL(1.0, finalWeights[0], tol); -// DOUBLES_EQUAL(1.0, finalWeights[1], tol); -// DOUBLES_EQUAL(1.0, finalWeights[2], tol); -// DOUBLES_EQUAL(0.0, finalWeights[3], tol); -// } -// { -// // if we set the threshold large, they are all inliers -// GncParams gncParams; -// gncParams.setKnownInliers(knownInliers); -// gncParams.setLossType(GncLossType::TLS); -// //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); -// auto gnc = GncOptimizer>(fg, initial, -// gncParams); -// gnc.setInlierCostThresholds(100.0); -// -// 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(0.0, finalWeights[3], tol); + } + + // nonconvexity with known inliers and known outliers + { + std::vector knownInliers; + knownInliers.push_back(2); + knownInliers.push_back(0); + + std::vector knownOutliers; + knownOutliers.push_back(3); + + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setKnownOutliers(knownOutliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setInlierCostThresholds(1.0); + 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], 1e-5); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + DOUBLES_EQUAL(0.0, finalWeights[3], tol); + } + + // only known outliers + { + std::vector knownOutliers; + knownOutliers.push_back(3); + + GncParams gncParams; + gncParams.setKnownOutliers(knownOutliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setInlierCostThresholds(1.0); + 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], 1e-5); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + DOUBLES_EQUAL(0.0, finalWeights[3], tol); + } } /* ************************************************************************* */ From d6a3171e671b2af2a67c547f161aa486cc3be2e0 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 10 May 2021 20:06:31 -0400 Subject: [PATCH 5/8] user can now also set the weights to initialize gnc! --- gtsam/nonlinear/GncOptimizer.h | 17 ++++++- tests/testGncOptimizer.cpp | 83 +++++++++++++++++++++++++++++++++- 2 files changed, 97 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 1a269468b5..db9e780e29 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -99,6 +99,10 @@ class GncOptimizer { "that are not in the factor graph to be known outliers."); } } + // initialize weights (if we don't have prior knowledge of inliers/outliers + // the weights are all initialized to 1. + weights_ = initializeWeightsFromKnownInliersAndOutliers(); + // set default barcSq_ (inlier threshold) double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_ setInlierCostThresholdsAtProbability(alpha); @@ -134,6 +138,17 @@ class GncOptimizer { } } + /** Set weights for each factor. This is typically not needed, but + * provides an extra interface for the user to initialize the weightst + * */ + void setWeights(const Vector w) { + if(w.size() != nfg_.size()){ + throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights" + " does not match the size of the factor graph."); + } + weights_ = w; + } + /// Access a copy of the internal factor graph. const NonlinearFactorGraph& getFactors() const { return nfg_; } @@ -167,8 +182,6 @@ class GncOptimizer { /// Compute optimal solution using graduated non-convexity. Values optimize() { - // start by assuming all measurements are inliers - weights_ = initializeWeightsFromKnownInliersAndOutliers(); NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_); BaseOptimizer baseOptimizer(graph_initial, state_); Values result = baseOptimizer.optimize(); diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 7160c32fd5..a3d1e4e9b0 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -660,7 +660,7 @@ TEST(GncOptimizer, barcsq_heterogeneousFactors) { } /* ************************************************************************* */ -TEST(GncOptimizer, setWeights) { +TEST(GncOptimizer, setInlierCostThresholds) { auto fg = example::sharedNonRobustFactorGraphWithOutliers(); Point2 p0(1, 0); @@ -839,6 +839,87 @@ TEST(GncOptimizer, knownInliersAndOutliers) { } } +/* ************************************************************************* */ +TEST(GncOptimizer, setWeights) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + // initialize weights to be the same + { + GncParams gncParams; + gncParams.setLossType(GncLossType::TLS); + + Vector weights = 0.5 * Vector::Ones(fg.size()); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setWeights(weights); + gnc.setInlierCostThresholds(1.0); + 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); + } + // try a more challenging initialization + { + GncParams gncParams; + gncParams.setLossType(GncLossType::TLS); + + Vector weights = Vector::Zero(fg.size()); + weights(2) = 1.0; + weights(3) = 1.0; // bad initialization: we say the outlier is inlier + // GNC can still recover (but if you omit weights(2) = 1.0, then it would fail) + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setWeights(weights); + gnc.setInlierCostThresholds(1.0); + 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); + } + // initialize weights and also set known inliers/outliers + { + GncParams gncParams; + std::vector knownInliers; + knownInliers.push_back(2); + knownInliers.push_back(0); + + std::vector knownOutliers; + knownOutliers.push_back(3); + gncParams.setKnownInliers(knownInliers); + gncParams.setKnownOutliers(knownOutliers); + + gncParams.setLossType(GncLossType::TLS); + + Vector weights = 0.5 * Vector::Ones(fg.size()); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + gnc.setWeights(weights); + gnc.setInlierCostThresholds(1.0); + 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); + } +} + /* ************************************************************************* */ int main() { TestResult tr; From 7d93e6fca0cce3a9b1413e998bbe64cfd75ff7a6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 12 May 2021 19:20:21 -0400 Subject: [PATCH 6/8] added comment on example interface --- gtsam_unstable/examples/GncPoseAveragingExample.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp index 7af577e18d..505a2db2cf 100644 --- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -12,6 +12,9 @@ /** * @file GncPoseAveragingExample.cpp * @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers + * You can run this example using: /GncPoseAveragingExample nrInliers nrOutliers + * e.g.,: /GncPoseAveragingExample 10 5 (if the numbers are not specified, default + * values nrInliers = 10 and nrOutliers = 10 are used) * @date May 8, 2021 * @author Luca Carlone */ From 5510b41e31d313a2080e4199fd5ec57242610a45 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 12 May 2021 19:22:16 -0400 Subject: [PATCH 7/8] amended --- gtsam_unstable/examples/GncPoseAveragingExample.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp index 505a2db2cf..ad96934c8b 100644 --- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -12,8 +12,8 @@ /** * @file GncPoseAveragingExample.cpp * @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers - * You can run this example using: /GncPoseAveragingExample nrInliers nrOutliers - * e.g.,: /GncPoseAveragingExample 10 5 (if the numbers are not specified, default + * You can run this example using: ./GncPoseAveragingExample nrInliers nrOutliers + * e.g.,: ./GncPoseAveragingExample 10 5 (if the numbers are not specified, default * values nrInliers = 10 and nrOutliers = 10 are used) * @date May 8, 2021 * @author Luca Carlone From d929c803836369256f91483e427a72957c321dea Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 22 May 2021 13:14:10 -0400 Subject: [PATCH 8/8] fixed formatting glitch --- 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 db9e780e29..eb353c53f9 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -424,7 +424,7 @@ class GncOptimizer { switch (params_.lossType) { case GncLossType::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 * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2);