Robust Incremental Smoothing and Mapping (riSAM)

gtsam/linear/tests/testNoiseModel.cpp

@@ -640,6 +640,23 @@ TEST(NoiseModel, robustFunctionHuber)
   DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8);
 }
 
+TEST(NoiseModel, robustFunctionHuberGraduated) {
+  const double k = 5.0, e1 = 1.0, e2 = 10.0;
+  const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
+  // Convex For large \mu
+  DOUBLES_EQUAL(1.0, huber->graduatedWeight(e1, 1e8), 1e-6);
+  DOUBLES_EQUAL(1.0, huber->graduatedWeight(e2, 1e8), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(huber->weight(e1), huber->graduatedWeight(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(huber->weight(e2), huber->graduatedWeight(e2, 1.0), 1e-6);
+  // Convex For large \mu
+  DOUBLES_EQUAL(0.5 * e1 * e1, huber->graduatedLoss(e1, 1e8), 1e-6);
+  DOUBLES_EQUAL(0.5 * e2 * e2, huber->graduatedLoss(e2, 1e8), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(huber->loss(e1), huber->graduatedLoss(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(huber->loss(e2), huber->graduatedLoss(e2, 1.0), 1e-6);
+}
+
 TEST(NoiseModel, robustFunctionCauchy)
 {
   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
@@ -656,6 +673,23 @@ TEST(NoiseModel, robustFunctionCauchy)
   DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8);
 }
 
+TEST(NoiseModel, robustFunctionCauchyGraduated) {
+  const double k = 5.0, e1 = 1.0, e2 = 10.0;
+  const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k);
+  // Convex For large \mu
+  DOUBLES_EQUAL(1.0, cauchy->graduatedWeight(e1, 1e8), 1e-6);
+  DOUBLES_EQUAL(1.0, cauchy->graduatedWeight(e2, 1e8), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(cauchy->weight(e1), cauchy->graduatedWeight(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(cauchy->weight(e2), cauchy->graduatedWeight(e2, 1.0), 1e-6);
+  // Convex For large \mu
+  DOUBLES_EQUAL(0.5 * e1 * e1, cauchy->graduatedLoss(e1, 1e8), 1e-6);
+  DOUBLES_EQUAL(0.5 * e2 * e2, cauchy->graduatedLoss(e2, 1e8), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(cauchy->loss(e1), cauchy->graduatedLoss(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(cauchy->loss(e2), cauchy->graduatedLoss(e2, 1.0), 1e-6);
+}
+
 TEST(NoiseModel, robustFunctionAsymmetricCauchy)
 {
   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
@@ -687,6 +721,67 @@ TEST(NoiseModel, robustFunctionGemanMcClure)
   DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8);
 }
 
+TEST(NoiseModel, robustFunctionGemanMcClureGraduatedScaled) {
+  const double k = 1.0, e1 = 1.0, e2 = 10.0;
+  const mEstimator::GemanMcClure::shared_ptr gmc =
+      mEstimator::GemanMcClure::Create(k);
+  // Convex For large \mu
+  DOUBLES_EQUAL(1.0, gmc->graduatedWeight(e1, 1e12), 1e-6);
+  DOUBLES_EQUAL(1.0, gmc->graduatedWeight(e2, 1e12), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(gmc->weight(e1), gmc->graduatedWeight(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(gmc->weight(e2), gmc->graduatedWeight(e2, 1.0), 1e-6);
+  // Convex For large \mu
+  DOUBLES_EQUAL(0.5 * e1 * e1, gmc->graduatedLoss(e1, 1e12), 1e-6);
+  DOUBLES_EQUAL(0.5 * e2 * e2, gmc->graduatedLoss(e2, 1e12), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(gmc->loss(e1), gmc->graduatedLoss(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(gmc->loss(e2), gmc->graduatedLoss(e2, 1.0), 1e-6);
+}
+
+/* ************************************************************************* */
+TEST(NoiseModel, robustFunctionGemanMcClureGraduatedScaleInvariant) {
+  mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(
+      1.0, mEstimator::GemanMcClure::GradScheme::SCALE_INVARIANT);
+  // At zero error is not dependent on mu
+  DOUBLES_EQUAL(0.0, gmc->graduatedLoss(0.0, 0.0), 1e-9);
+  CHECK(assert_equal(0.0, gmc->graduatedLoss(0.0, 0.5), 1e-9));
+  CHECK(assert_equal(0.0, gmc->graduatedLoss(0.0, 1.0), 1e-9));
+
+  // For Mu = 0.0 error is quadratic
+  DOUBLES_EQUAL(0.0025, gmc->graduatedLoss(0.1, 0.0), 1e-9);
+  DOUBLES_EQUAL(0.4225, gmc->graduatedLoss(1.3, 0.0), 1e-9);
+  DOUBLES_EQUAL(38.750625, gmc->graduatedLoss(12.45, 0.0), 1e-9);
+
+  // For 0.0 < Mu Error depends on shape
+  DOUBLES_EQUAL(0.00454545454, gmc->graduatedLoss(0.1, 0.5), 1e-9);
+  DOUBLES_EQUAL(0.36739130434, gmc->graduatedLoss(1.3, 0.5), 1e-9);
+  DOUBLES_EQUAL(5.76217472119, gmc->graduatedLoss(12.45, 0.5), 1e-9);
+
+  // For Mu == 1.0 error depends is Geman-Mcclure
+  DOUBLES_EQUAL(0.00495049504, gmc->graduatedLoss(0.1, 1.0), 1e-9);
+  DOUBLES_EQUAL(0.31412639405, gmc->graduatedLoss(1.3, 1.0), 1e-9);
+  DOUBLES_EQUAL(0.49679492315, gmc->graduatedLoss(12.45, 1.0), 1e-9);
+
+  // At Mu = 0 weights are identical at 0.5
+  DOUBLES_EQUAL(0.5, gmc->graduatedWeight(0.1, 0.0), 1e-9);
+  DOUBLES_EQUAL(0.5, gmc->graduatedWeight(1.3, 0.0), 1e-9);
+  DOUBLES_EQUAL(0.5, gmc->graduatedWeight(12.45, 0.0), 1e-9);
+
+  // At Mu = 1 weights are higher for low error
+  DOUBLES_EQUAL(0.9802960494, gmc->graduatedWeight(0.1, 1.0), 1e-9);
+  DOUBLES_EQUAL(0.13819598955, gmc->graduatedWeight(1.3, 1.0), 1e-9);
+  DOUBLES_EQUAL(0.00004109007, gmc->graduatedWeight(12.45, 1.0), 1e-9);
+
+  // At Mu = 1 large residuals have ~0 weight
+  DOUBLES_EQUAL(0.0, gmc->graduatedWeight(2000.0, 1.0), 1e-9);
+
+  // Across Mu residual of 0 will have weight = 1
+  DOUBLES_EQUAL(1.0, gmc->graduatedWeight(0.0, 0.1), 1e-9);
+  DOUBLES_EQUAL(1.0, gmc->graduatedWeight(0.0, 0.5), 1e-9);
+  DOUBLES_EQUAL(1.0, gmc->graduatedWeight(0.0, 0.8), 1e-9);
+}
+
 TEST(NoiseModel, robustFunctionTLS)
 {
   const double k = 4.0, error1 = 0.5, error2 = 10.0, error3 = -10.0, error4 = -0.5;
@@ -702,6 +797,24 @@ TEST(NoiseModel, robustFunctionTLS)
   DOUBLES_EQUAL(0.1250, tls->loss(error4), 1e-8);
 }
 
+TEST(NoiseModel, robustFunctionTruncatedLeastSquaresGraduated) {
+  const double k = 5.0, e1 = 1.0, e2 = 10.0;
+  const mEstimator::TruncatedLeastSquares::shared_ptr tls =
+      mEstimator::TruncatedLeastSquares::Create(k);
+  // Convex For large \mu
+  DOUBLES_EQUAL(1.0, tls->graduatedWeight(e1, 1e12), 1e-6);
+  DOUBLES_EQUAL(1.0, tls->graduatedWeight(e2, 1e12), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(tls->weight(e1), tls->graduatedWeight(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(tls->weight(e2), tls->graduatedWeight(e2, 1.0), 1e-6);
+  // Convex For large \mu
+  DOUBLES_EQUAL(0.5 * e1 * e1, tls->graduatedLoss(e1, 1e12), 1e-6);
+  DOUBLES_EQUAL(0.5 * e2 * e2, tls->graduatedLoss(e2, 1e12), 1e-6);
+  // Standard for \mu = 1
+  DOUBLES_EQUAL(tls->loss(e1), tls->graduatedLoss(e1, 1.0), 1e-6);
+  DOUBLES_EQUAL(tls->loss(e2), tls->graduatedLoss(e2, 1.0), 1e-6);
+}
+
 TEST(NoiseModel, robustFunctionWelsch)
 {
   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
@@ -809,9 +922,11 @@ TEST(NoiseModel, robustNoiseGemanMcClure)
   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::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar),
-    Unit::Create(2));
+  const Robust::shared_ptr robust =
+      Robust::Create(mEstimator::GemanMcClure::Create(
+                         k, mEstimator::GemanMcClure::GradScheme::STANDARD,
+                         mEstimator::GemanMcClure::Scalar),
+                     Unit::Create(2));
 
   robust->WhitenSystem(A, b);
 
@@ -908,6 +1023,7 @@ TEST(NoiseModel, robustNoiseCustomHuber) {
                            const auto abs_e = std::abs(e);
                            return abs_e <= k ? abs_e * abs_e / 2.0 : k * abs_e - k * k / 2.0;
                          },
+                         std::nullopt, std::nullopt,
                          noiseModel::mEstimator::Custom::Scalar, "Huber"),
                      Unit::Create(2));
 

gtsam/nonlinear/GncOptimizer.h

@@ -515,7 +515,7 @@ class GncOptimizer {
         for (size_t k = 0; k < nfg_.size(); k++) {
           if (needsWeightUpdate(factorTypes_[k])) {
             double u2_k = nfg_[k]->error(currentEstimate);  // squared (and whitened) residual
-            weights[k] = noiseModel::mEstimator::GemanMcClure::Weight(u2_k, mu * barcSq_[k]);
+            weights[k] = noiseModel::mEstimator::GemanMcClure::Weight(mu * barcSq_[k], u2_k);
           }
         }
         return weights;
@@ -528,7 +528,7 @@ class GncOptimizer {
               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);
+                auto w = noiseModel::mEstimator::TruncatedLeastSquares::GNCWeight(u2_k, lowerbound, upperbound);
                 if (w) {
                   weights[k] = *w;
                 }
@@ -541,7 +541,7 @@ class GncOptimizer {
               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);
+                auto w = noiseModel::mEstimator::TruncatedLeastSquares::GNCWeight(u2_k, lowerbound, upperbound);
                 if (w) {
                   weights[k] = *w;
                 }

gtsam/nonlinear/doc/RISAM.ipynb

@@ -0,0 +1,127 @@
+{
+  "cells": [
+    {
+      "cell_type": "markdown",
+      "id": "c950beef",
+      "metadata": {},
+      "source": [
+        "# RISAM - Robust Incremental Smoothing and Mapping\n",
+        "\n",
+        "## Overview\n",
+        "\n",
+        "The `RISAM` class in GTSAM is designed to perform online robust optimization using an incrementalized version of [Graduated Non-Convexity](GNCOptimizer.ipynb) built on the [ISAM2](ISAM2.ipynb) algorithm. This method is intended for scenarios where the incremental optimization problem is affected by outliers. In cases where all measurements are known to be inliers `RISAM` functions identically to `ISAM2`, however, when potential outliers are incorporated `RISAM` applies an incremental GNC step to the effected problem improving robustness over standard ISAM2 but preventing sensitivity to initialization found in M-Estimation approaches.\n",
+        "\n",
+        "Like the `GNCOptimizer`, `RISAM` leverages a robust cost function $\\rho(e)$, where $e$ is the error term. The goal is to minimize the sum of these robust costs over all measurements:\n",
+        "\n",
+        "$$\n",
+        "\\min_x \\sum_i \\rho(e_i(x))\n",
+        "$$\n",
+        "\n",
+        "Unlike the batch setting that `GNCOptimzier` is used for, `RISAM` targets the incremental problem where we incrementally incorporate measurements online.\n",
+        "\n",
+        "$$\n",
+        "\\min_{x^t} \\sum_i \\rho(e_i(x^t))\n",
+        "$$\n",
+        "\n",
+        "Where we re-solve only a small subproblem at each step that is affected by the new measurements. `RISAM` solves this sub-problem robustly by solving a continuation of problems defined by graduated robust kernel $\\rho(e, \\mu)$ where the control parameter $\\mu$ smoothly transitions the kernel from quadratic ($x^2$) to a robust loss.\n",
+        "\n",
+        "$$\n",
+        "\\rho(e^t, \\mu)\n",
+        "$$\n",
+        "\n",
+        "By starting with non-robust error `RISAM` better handles poor-initialization, and by transitioning to the final robust loss `RISAM` removes the influence of outliers.\n",
+        "\n",
+        "Key features:\n",
+        "\n",
+        "- **Online Robust Optimization**: `RISAM` is designed to support incremental optimization problems with outliers, using a robust cost function that can mitigate their effects.\n",
+        "- **Incremental Graduated Non-Convexity**: This technique allows the optimizer to solve each incremental subproblem with a convex problem and gradually transform it into the original non-convex problem, which helps in avoiding local minima.\n",
+        "\n",
+        "## Key Methods + Classes\n",
+        "\n",
+        "`RISAM` is designed to be a drop-in replacement for ISAM2. To see details on its key methods see [ISAM2.ipynb](ISAM2.ipynb).\n",
+        "\n",
+        "Additional Key Methods:\n",
+        "* `getOutliers`: Returns the set of measurements currently classified as outliers.\n",
+        "\n",
+        "Additional Key Helpers + Classes\n",
+        "* `make_graduated`: Constructs any factor as a `GraduatedFactor` which identifies factors as possible outliers to `RISAM`. All other factors are treated as known inliers.\n",
+        "* `SIGKernel`: The suggested kernel to use with `GraduatedFactors` provides stable optimization performance.\n",
+        "\n",
+        "## Parameters\n",
+        "\n",
+        "The `RISAM::Parameters` class defines parameters specific to `RISAM`:\n",
+        "\n",
+        "| Parameter | Type | Default Value | Description |\n",
+        "|-----------|------|---------------|-------------|\n",
+        "| isam2_params | ISAM2Params | ISAM2Params() | The parameters for the encapsulated ISAM2 optimizer. It is recommended to use DogLegLineSearch for optimization. |\n",
+        "| increment_outlier_mu | bool | true | Whether to increment the initial value of $\\mu$ used for each incremental GNC update over time as certainty of their inlier/outlier status increases. |\n",
+        "| outlier_mu_chisq_upper_bound | double | 0.95 | The $\\chi^2$ threshold for factor residual to consider it an outlier for $\\mu_{init}$ updates.. |\n",
+        "| outlier_mu_chisq_lower_bound | double | 0.25| The $\\chi^2$ threshold for factor residual to consider it strong inlier for $\\mu_{init}$ updates. |\n",
+        "| outlier_mu_avg_var_convergence_thresh | double | 0.01 | The threshold average variable delta to initiate $\\mu_{init}$ updates. |\n",
+        "| number_extra_iters | size_t | 1 | The number of extra `ISAM2::updates` called internally at each iteration after converging to the fully robust problem. |\n",
+        "\n",
+        "## Usage Considerations\n",
+        "\n",
+        "- **Outlier Rejection**: `RISAM` is particularly effective in online scenarios with significant outlier presence, such as online SLAM.\n",
+        "- **Trust Region Optimization**: While `RISAM` can use any underlying optimization step methods supported by ISAM2 it is strongly recommended to use `DoglegLineSearch` as it accounts for the changes in problem structure (convexity changes) and prevents divergence through trust region steps.\n",
+        "\n",
+        "## Files\n",
+        "\n",
+        "- [RISAM.h](https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RISAM.h)\n",
+        "- [RISAMGraduatedFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RISAMGraduatedFactor.h)\n",
+        "- [RISAMGraduatedKernel.h](https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RISAMGraduatedKernel.h)"
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "id": "colab_button",
+      "metadata": {},
+      "source": [
+        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/nonlinear/doc/GncOptimizer.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "id": "license_cell",
+      "metadata": {
+        "tags": [
+          "remove-cell"
+        ]
+      },
+      "source": [
+        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
+        "Atlanta, Georgia 30332-0415\n",
+        "All Rights Reserved\n",
+        "\n",
+        "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
+        "\n",
+        "See LICENSE for the license information"
+      ]
+    },
+    {
+      "cell_type": "code",
+      "execution_count": null,
+      "id": "colab_import",
+      "metadata": {
+        "tags": [
+          "remove-cell"
+        ]
+      },
+      "outputs": [],
+      "source": [
+        "try:\n",
+        "    import google.colab\n",
+        "    %pip install --quiet gtsam-develop\n",
+        "except ImportError:\n",
+        "    pass"
+      ]
+    }
+  ],
+  "metadata": {
+    "language_info": {
+      "name": "python"
+    }
+  },
+  "nbformat": 4,
+  "nbformat_minor": 5
+}