Merge pull request #2417 from hkhanuja/feature/tlsloss

TLS loss and Rene Vidal TLS implementation

Author: dellaert

gtsam/linear/LossFunctions.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/linear/LossFunctions.h>
 
 #include <iostream>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -346,6 +347,49 @@ GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme rew
   return shared_ptr(new GemanMcClure(c, reweight));
 }
 
+/* ************************************************************************* */
+// TruncatedLeastSquares
+/* ************************************************************************* */
+
+TruncatedLeastSquares::TruncatedLeastSquares(double c, const ReweightScheme reweight)
+  : Base(reweight), c_(c), csquared_(c * c) {
+  if (c_ <= 0) {
+    throw runtime_error("mEstimator TruncatedLeastSquares takes only positive double in constructor.");
+  }
+}
+
+double TruncatedLeastSquares::weight(double distance) const {
+  const auto w = Weight(distance * distance, csquared_, csquared_);
+  return w.value();
+}
+
+std::optional<double> TruncatedLeastSquares::Weight(double distance2, double lowerbound, double upperbound) {
+  if (distance2 <= lowerbound) return 1.0;
+  if (distance2 >= upperbound) return 0.0;
+  return std::nullopt;
+}
+
+double TruncatedLeastSquares::loss(double distance) const {
+  if (std::abs(distance) <= c_) {
+    return 0.5 * distance * distance;
+  }
+  return 0.5 * csquared_;
+}
+
+void TruncatedLeastSquares::print(const std::string &s="") const {
+  std::cout << s << ": TLS (" << c_ << ")" << std::endl;
+}
+
+bool TruncatedLeastSquares::equals(const Base &expected, double tol) const {
+  const TruncatedLeastSquares* p = dynamic_cast<const TruncatedLeastSquares*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(c_ - p->c_) < tol;
+}
+
+TruncatedLeastSquares::shared_ptr TruncatedLeastSquares::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new TruncatedLeastSquares(c, reweight));
+}
+
 /* ************************************************************************* */
 // DCS
 /* ************************************************************************* */

gtsam/linear/LossFunctions.h

@@ -20,6 +20,7 @@
 
 #pragma once
 
+#include <optional>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/dllexport.h>
@@ -406,6 +407,56 @@ class GTSAM_EXPORT GemanMcClure : public Base {
 #endif
 };
 
+/** Truncated Least Squares (TLS) robust error model.
+ *
+ *  This model has a scalar parameter "c" (threshold).
+ *
+ * - Loss       \rho(x) = 0.5 x^2  if |x|<=c, 0.5 c^2 otherwise
+ * - Derivative \phi(x) = x        if |x|<=c, 0 otherwise
+ * - Weight     w(x) = \phi(x)/x = 1 if |x|<=c, 0 otherwise
+ */
+class GTSAM_EXPORT TruncatedLeastSquares : public Base {
+ public:
+  typedef std::shared_ptr<TruncatedLeastSquares> shared_ptr;
+
+  TruncatedLeastSquares(double c = 1.0, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double c, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
+  /** @brief A static helper function to compute the TLS robust weight.
+   * The static function takes the squared value of the residual, the squared lower bound, the squared upper bound.
+   * This helper returns a optional<double> because it is also used for GNC, and we encounter transition weight cases,
+   * where the weight is not strictly binary (0 or 1) when the residual is within the transition region between inliers and outliers.
+   * The weight member function now calls the this function.
+   * While the member function takes the residual as input, it passes x², c² and c² to the static helper.
+   * 
+   * @param distance2 Squared residual magnitude.
+   * @param lowerbound Squared lower bound.
+   * @param upperbound Squared upper bound.
+   * @return Weight w(x) is {0, 1} or None if the residual is between lowerbound and upperbound.
+   */
+   static std::optional<double> Weight(double distance2, double lowerbound, double upperbound);
+
+ protected:
+  double c_;
+  double csquared_;
+
+ private:
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(c_);
+    ar &BOOST_SERIALIZATION_NVP(csquared_);
+  }
+#endif
+};
+
 /** DCS implements the Dynamic Covariance Scaling robust error model
  *  from the paper Robust Map Optimization (Agarwal13icra).
  *

gtsam/linear/linear.i

@@ -176,6 +176,21 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
   double loss(double error) const;
 };
 
+virtual class TruncatedLeastSquares: gtsam::noiseModel::mEstimator::Base {
+  TruncatedLeastSquares(double c);
+  TruncatedLeastSquares(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
+  static gtsam::noiseModel::mEstimator::TruncatedLeastSquares* Create(double c);
+  static gtsam::noiseModel::mEstimator::TruncatedLeastSquares* Create(
+      double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+
 virtual class DCS: gtsam::noiseModel::mEstimator::Base {
   DCS(double c);
   DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);

gtsam/linear/tests/testNoiseModel.cpp

@@ -687,6 +687,21 @@ TEST(NoiseModel, robustFunctionGemanMcClure)
   DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8);
 }
 
+TEST(NoiseModel, robustFunctionTLS)
+{
+  const double k = 4.0, error1 = 0.5, error2 = 10.0, error3 = -10.0, error4 = -0.5;
+  const mEstimator::TruncatedLeastSquares::shared_ptr tls = mEstimator::TruncatedLeastSquares::Create(k);
+  DOUBLES_EQUAL(1.0, tls->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.0, tls->weight(error2), 1e-8);
+  DOUBLES_EQUAL(0.0, tls->weight(error3), 1e-8);
+  DOUBLES_EQUAL(1.0, tls->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.1250, tls->loss(error1), 1e-8);
+  DOUBLES_EQUAL(8.0, tls->loss(error2), 1e-8);
+  DOUBLES_EQUAL(8.0, tls->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.1250, tls->loss(error4), 1e-8);
+}
+
 TEST(NoiseModel, robustFunctionWelsch)
 {
   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
@@ -816,6 +831,30 @@ TEST(NoiseModel, robustNoiseGemanMcClure)
   DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8);
 }
 
+TEST(NoiseModel, robustNoiseTLS)
+{
+  const double k = 1.0, error1 = 1.0, error2 = 100.0;
+  const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
+  Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
+  Vector b = Vector2(error1, error2);
+  const Robust::shared_ptr robust = Robust::Create(
+    mEstimator::TruncatedLeastSquares::Create(k, mEstimator::TruncatedLeastSquares::Scalar),
+    Unit::Create(2));
+
+  robust->WhitenSystem(A, b);
+
+  const double sqrt_weight_error1 = 1.0;
+  const double sqrt_weight_error2 = 0.0;
+
+  DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8);
+  DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8);
+
+  DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8);
+  DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8);
+  DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8);
+  DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8);
+}
+
 TEST(NoiseModel, robustNoiseDCS)
 {
   const double k = 1.0, error1 = 1.0, error2 = 100.0;

gtsam/nonlinear/GncOptimizer.h

@@ -26,6 +26,8 @@
 
 #pragma once
 
+#include <algorithm>
+
 #include <gtsam/linear/LossFunctions.h>
 #include <gtsam/nonlinear/GncParams.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -236,6 +238,7 @@ class GncOptimizer {
 
   /// Compute optimal solution using graduated non-convexity.
   Values optimize() {
+    validateLossSchedulerCombination();
     NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
     BaseOptimizer baseOptimizer(
         graph_initial, state_, params_.baseOptimizerParams);
@@ -328,6 +331,18 @@ class GncOptimizer {
     return result;
   }
 
+  void validateLossSchedulerCombination() const {
+    if (params_.lossType == GncLossType::GM &&
+        params_.scheduler != GncScheduler::Linear) {
+      throw std::runtime_error(
+          "GncOptimizer::optimize: scheduler must be Linear for GM.");
+    }
+    if (params_.lossType == GncLossType::TLS) {
+      // Linear and SuperLinear are both valid for TLS.
+      return;
+    }
+  }
+
   /// Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
   double initializeMu() const {
 
@@ -381,7 +396,17 @@ class GncOptimizer {
         return std::max(1.0, mu / params_.muStep);
       case GncLossType::TLS:
         // increases mu at each iteration (original cost is recovered for mu -> inf)
-        return mu * params_.muStep;
+        switch (params_.scheduler) {
+          case GncScheduler::SuperLinear: {
+            if (mu < 1) return std::min(std::sqrt(mu) * params_.muStep, params_.muMax);
+            return std::min(mu * params_.muStep, params_.muMax);
+          }
+          case GncScheduler::Linear: {
+            return mu * params_.muStep;
+          }
+          default:
+            throw std::runtime_error("GncOptimizer::updateMu: unknown scheduler type.");
+        }
       default:
         throw std::runtime_error(
             "GncOptimizer::updateMu: called with unknown loss type.");
@@ -495,17 +520,39 @@ class GncOptimizer {
         }
         return weights;
       }
-      case GncLossType::TLS: {  // use eq (14) in GNC paper
+      case GncLossType::TLS: {
         for (size_t k = 0; k < nfg_.size(); k++) {
           if (needsWeightUpdate(factorTypes_[k])) {
             double u2_k = nfg_[k]->error(currentEstimate);  // squared (and whitened) residual
-            double upperbound = (mu + 1) / mu * barcSq_[k];
-            double lowerbound = mu / (mu + 1) * barcSq_[k];
-            weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
-            if (u2_k >= upperbound || weights[k] < 0) {
-              weights[k] = 0;
-            } else if (u2_k <= lowerbound || weights[k] > 1) {
-              weights[k] = 1;
+            switch (params_.scheduler) {
+              case GncScheduler::SuperLinear: {
+                double lowerbound = barcSq_[k];
+                double upperbound = ((mu + 1.0) * (mu + 1.0) / (mu * mu)) * barcSq_[k];
+                auto w = noiseModel::mEstimator::TruncatedLeastSquares::Weight(u2_k, lowerbound, upperbound);
+                if (w) {
+                  weights[k] = *w;
+                }
+                else {
+                  double transition_weight = std::sqrt(barcSq_[k] / u2_k) * (mu + 1.0)  - mu;
+                  weights[k] = std::clamp(transition_weight, 0.0, 1.0);
+                }
+                break;
+              }
+              case GncScheduler::Linear: {  // use eq (14) in GNC paper
+                double upperbound = ((mu + 1.0) / mu) * barcSq_[k];
+                double lowerbound = (mu / (mu + 1.0)) * barcSq_[k];
+                auto w = noiseModel::mEstimator::TruncatedLeastSquares::Weight(u2_k, lowerbound, upperbound);
+                if (w) {
+                  weights[k] = *w;
+                }
+                else {
+                  double transition_weight = std::sqrt(barcSq_[k] * mu * (mu + 1.0) / u2_k) - mu;
+                  weights[k] = std::clamp(transition_weight, 0.0, 1.0);
+                }
+                break;
+              }
+              default:
+                throw std::runtime_error("GncOptimizer::calculateWeights: unknown scheduler type.");
             }
           }
         }

gtsam/nonlinear/GncParams.h

@@ -38,6 +38,13 @@ enum GncLossType {
   TLS /*Truncated least squares*/
 };
 
+/// Choice of GNC scheduling strategy.
+/// SuperLinear reference https://openaccess.thecvf.com/content/CVPR2023/papers/Peng_On_the_Convergence_of_IRLS_and_Its_Variants_in_Outlier-Robust_CVPR_2023_paper.pdf
+enum class GncScheduler {
+  Linear,
+  SuperLinear
+};
+
 template<class BaseOptimizerParameters>
 class GncParams {
  public:
@@ -67,10 +74,12 @@ class GncParams {
   BaseOptimizerParameters baseOptimizerParams;  ///< Optimization parameters used to solve the weighted least squares problem at each GNC iteration
   /// any other specific GNC parameters:
   GncLossType lossType = TLS;  ///< Default loss
+  GncScheduler scheduler = GncScheduler::Linear;  ///< Default scheduler
   size_t maxIterations = 100;  ///<  Maximum number of iterations
   double muStep = 1.4;  ///< Multiplicative factor to reduce/increase the mu in gnc
   double relativeCostTol = 1e-5;  ///< If relative cost change is below this threshold, stop iterating
   double weightsTol = 1e-4;  ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
+  double muMax = 1e16;  ///< Maximum value of mu in GNC, acts as a cap (only for TLS)
   Verbosity verbosity = SILENT;  ///< Verbosity level
   bool allowNonNoiseModelFactors = false;  ///< If true, factors without noise model are not reweighted and not not included in mu calculation
 
@@ -86,6 +95,11 @@ class GncParams {
     lossType = type;
   }
 
+  /// Set the scheduler type.
+  void setScheduler(const GncScheduler s) {
+    scheduler = s;
+  }
+
   /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended).
   void setMaxIterations(const size_t maxIter) {
     std::cout
@@ -147,6 +161,7 @@ class GncParams {
     return baseOptimizerParams.equals(other.baseOptimizerParams)
         && lossType == other.lossType && maxIterations == other.maxIterations
         && std::fabs(muStep - other.muStep) <= tol
+        && scheduler == other.scheduler
         && verbosity == other.verbosity && knownInliers == other.knownInliers
         && knownOutliers == other.knownOutliers
         && allowNonNoiseModelFactors == other.allowNonNoiseModelFactors;
@@ -165,6 +180,16 @@ class GncParams {
       default:
         throw std::runtime_error("GncParams::print: unknown loss type.");
     }
+    switch (scheduler) {
+      case GncScheduler::Linear:
+        std::cout << "scheduler: Linear" << "\n";
+        break;
+      case GncScheduler::SuperLinear:
+        std::cout << "scheduler: SuperLinear" << "\n";
+        break;
+      default:
+        throw std::runtime_error("GncParams::print: unknown scheduler type.");
+    }
     std::cout << "maxIterations: " << maxIterations << "\n";
     std::cout << "muStep: " << muStep << "\n";
     std::cout << "relativeCostTol: " << relativeCostTol << "\n";

gtsam/nonlinear/nonlinear.i

@@ -300,6 +300,11 @@ enum GncLossType {
   TLS /*Truncated least squares*/
 };
 
+enum GncScheduler {
+  Linear,
+  SuperLinear
+};
+
 template<PARAMS>
 virtual class GncParams {
   GncParams(const PARAMS& baseOptimizerParams);
@@ -314,6 +319,7 @@ virtual class GncParams {
   gtsam::This::IndexVector knownInliers;
   gtsam::This::IndexVector knownOutliers;
   bool allowNonNoiseModelFactors;
+  gtsam::GncScheduler scheduler;
 
   void setLossType(const gtsam::GncLossType type);
   void setMaxIterations(const size_t maxIter);
@@ -324,6 +330,7 @@ virtual class GncParams {
   void setKnownInliers(const gtsam::This::IndexVector& knownIn);
   void setKnownOutliers(const gtsam::This::IndexVector& knownOut);
   void setAllowNonNoiseModelFactors(bool allow);
+  void setScheduler(const gtsam::GncScheduler scheduler);
   void print(const string& str = "GncParams: ") const;
 
   enum Verbosity {