- fixed stopping conditions

- handled degenerate case in mu initialization
- set TLS as default
- added more unit tests

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<size_t> knownInliers = std::vector<size_t>(); /* 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 = nfg_.error(result);
-    double prev_cost = cost;
+    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
@@ -216,15 +222,20 @@ public:
       if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) {
         std::cout << "GNC Optimizer stopped because maximum residual at "
             "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)) {
+      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;
-        break;
+      std::cout << "current cost: " << cost << std::endl;
-      }
-
-      // update mu
-      mu = updateMu(mu);
     }
     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,52 +317,67 @@ 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 {
+    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:
-      return std::fabs(cost - prev_cost) < params_.relativeCostTol;
+        weightsConverged = true;
+        for(size_t i=0; i<weights.size(); i++){
+          if( std::fabs ( weights[i] - std::round(weights[i]) ) > params_.weightsTol ){
+            weightsConverged = false;
+            break;
+          }
+        }
+        break;
       default:
         throw std::runtime_error(
-          "GncOptimizer::checkMuConvergence: called with unknown loss type.");
+            "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) {
+    return checkCostConvergence(cost,prev_cost) ||
-    case GncParameters::GM:
+        checkWeightsConvergence(weights) ||
-      return checkMuConvergence(mu);
+        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 {

tests/testGncOptimizer.cpp

@@ -66,7 +66,7 @@ TEST(GncOptimizer, gncParamsConstructor) {
 
   // change something at the gncParams level
   GncParams<GaussNewtonParams> gncParams2c(gncParams2b);
-  gncParams2c.setLossType(GncParams<GaussNewtonParams>::RobustLossType::TLS);
+  gncParams2c.setLossType(GncParams<GaussNewtonParams>::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<LevenbergMarquardtParams> gncParams(lmParams);
   gncParams.setLossType(
@@ -204,6 +205,112 @@ TEST(GncOptimizer, checkMuConvergenceGM) {
   double mu = 1.0;
   CHECK(gnc.checkMuConvergence(mu));
   }
+  {
+  LevenbergMarquardtParams lmParams;
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(
+      GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(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<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setRelativeCostTol(0.49);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(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<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setRelativeCostTol(0.51);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(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<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(
+      GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
+
+  Vector weights = Vector::Ones(fg.size());
+  CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM
+  }
+  {
+  LevenbergMarquardtParams lmParams;
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(
+      GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
+
+  Vector weights = Vector::Ones(fg.size());
+  // weights are binary, so checkWeightsConvergence = true
+  CHECK(gnc.checkWeightsConvergence(weights));
+  }
+  {
+  LevenbergMarquardtParams lmParams;
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(
+      GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(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<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(
+      GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
+  gncParams.setWeightsTol(0.1);
+  auto gnc =
+      GncOptimizer<GncParams<LevenbergMarquardtParams>>(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));
+  }
+}
 
 /* ************************************************************************* */
 TEST(GncOptimizer, checkConvergenceTLS) {
@@ -455,9 +562,12 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
   knownInliers.push_back(2);
 
   // nonconvexity with known inliers
+  {
   GncParams<GaussNewtonParams> gncParams;
   gncParams.setKnownInliers(knownInliers);
-  // gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::VALUES);
+  gncParams.setLossType(
+        GncParams<GaussNewtonParams>::RobustLossType::GM);
+  //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::SUMMARY);
   auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
 
   Values gnc_result = gnc.optimize();
@@ -469,6 +579,45 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
   DOUBLES_EQUAL(1.0, finalWeights[1], tol);
   DOUBLES_EQUAL(1.0, finalWeights[2], tol);
   }
+  {
+  GncParams<GaussNewtonParams> gncParams;
+  gncParams.setKnownInliers(knownInliers);
+  gncParams.setLossType(
+        GncParams<GaussNewtonParams>::RobustLossType::TLS);
+  // gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::SUMMARY);
+  auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
+
+  Values gnc_result = gnc.optimize();
+  CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(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<GaussNewtonParams> gncParams;
+  gncParams.setKnownInliers(knownInliers);
+  gncParams.setLossType(
+        GncParams<GaussNewtonParams>::RobustLossType::TLS);
+  //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::VALUES);
+  gncParams.setInlierCostThreshold( 100.0 );
+  auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
+
+  Values gnc_result = gnc.optimize();
+  CHECK(assert_equal(Point2(0.25, 0.0), gnc_result.at<Point2>(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);
+  }
+}
 
 /* ************************************************************************* */
 TEST(GncOptimizer, optimizeSmallPoseGraph) {