Robust Incremental Smoothing and Mapping (riSAM)

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
+}