diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index e1efe7132a..eb353c53f9 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -74,6 +74,35 @@ class GncOptimizer { } } + // check that known inliers and outliers make sense: + 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(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."); + } + // 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 ){ // 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 ){ // 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."); + } + } + // 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); @@ -109,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_; } @@ -132,26 +172,39 @@ 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; // known to be outliers + } + return weights; + } + /// Compute optimal solution using graduated non-convexity. Values optimize() { - // start by assuming all measurements are inliers - weights_ = Vector::Ones(nfg_.size()); - 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 // 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 (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." << 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,17 +403,21 @@ 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; 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())); // update weights of known inlier/outlier measurements 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/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp new file mode 100644 index 0000000000..ad96934c8b --- /dev/null +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * 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 + */ + +#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 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 + 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); + // 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; +} diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 981e475ca1..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); @@ -749,6 +749,177 @@ 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); + + // 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)); + + // 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); + } +} + +/* ************************************************************************* */ +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;