Gnc enhancements

gtsam/nonlinear/GncOptimizer.h

@@ -74,6 +74,35 @@ class GncOptimizer {
       }
     }
 
+    // check that known inliers and outliers make sense:
+    std::vector<size_t> 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<size_t> allWeights;
     for (size_t k = 0; k < nfg_.size(); k++) {
       allWeights.push_back(k);
     }
+    std::vector<size_t> knownWeights;
+    std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
+                   params_.knownOutliers.begin(), params_.knownOutliers.end(),
+                   std::inserter(knownWeights, knownWeights.begin()));
+
     std::vector<size_t> 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

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<size_t> knownInliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are inliers
+  std::vector<size_t> knownOutliers = std::vector<size_t>();  ///< 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,16 +113,30 @@ class GncParams {
    * only apply GNC to prune outliers from the loop closures.
    * */
   void setKnownInliers(const std::vector<size_t>& 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<size_t>& knownOut) {
+    for (size_t i = 0; i < knownOut.size(); i++){
+      knownOutliers.push_back(knownOut[i]);
+    }
+    std::sort(knownOutliers.begin(), knownOutliers.end());
   }
 
   /// Equals.
   bool equals(const GncParams& other, double tol = 1e-9) const {
     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);
   }
 };

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 <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/GncOptimizer.h>
+
+#include <string>
+#include <fstream>
+#include <iostream>
+#include <random>
+#include <boost/lexical_cast.hpp>
+
+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<double> uniform(-10, 10);
+  normal_distribution<double> 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<nrInliers; i++){
+    Vector6 poseNoise;
+    for(size_t i = 0; i < 6; ++i){
+      poseNoise(i) = normalInliers(rng);
+    }
+    Pose3 poseMeasurement = gtPose.retract(poseNoise);
+    graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
+  }
+
+  // create outliers
+  for(size_t i=0; i<nrOutliers; i++){
+    Vector6 poseNoise;
+    for(size_t i = 0; i < 6; ++i){
+      poseNoise(i) = uniform(rng);
+    }
+    Pose3 poseMeasurement = gtPose.retract(poseNoise);
+    graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
+  }
+
+  GncParams<LevenbergMarquardtParams> gncParams;
+  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(graph,
+      initial,
+      gncParams);
+
+  Values estimate = gnc.optimize();
+  Pose3 poseError = gtPose.between( estimate.at<Pose3>(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;
+}