gtsam/gtsam/slam/doc/SmartProjectionParams.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# SmartProjectionParams"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`SmartProjectionParams` is a structure used to configure the behavior of \"smart\" factors like `SmartProjectionFactor`, `SmartProjectionPoseFactor`, and `SmartStereoFactor`.\n",
        "These factors implicitly manage the triangulation of a 3D point observed by multiple cameras.\n",
        "\n",
        "Parameters include:\n",
        "- **`linearizationMode`**: Determines the type of linear factor produced when `.linearize()` is called.\n",
        "  - `HESSIAN` (Default): Creates a `RegularHessianFactor` by marginalizing out the point.\n",
        "  - `IMPLICIT_SCHUR`: Creates a `RegularImplicitSchurFactor`.\n",
        "  - `JACOBIAN_Q`: Creates a `JacobianFactorQ`.\n",
        "  - `JACOBIAN_SVD`: Creates a `JacobianFactorSVD`.\n",
        "- **`degeneracyMode`**: How to handle cases where triangulation fails or is ill-conditioned.\n",
        "  - `IGNORE_DEGENERACY` (Default): Factor might become ill-conditioned.\n",
        "  - `ZERO_ON_DEGENERACY`: Return a zero-information factor (Hessian/Jacobian) if triangulation fails.\n",
        "  - `HANDLE_INFINITY`: Treat the point as being at infinity (uses `Unit3` representation).\n",
        "- **`triangulation`**: A `gtsam.TriangulationParameters` struct containing parameters for the `triangulateSafe` function:\n",
        "    - `rankTolerance`: Rank tolerance threshold for SVD during triangulation.\n",
        "    - `enableEPI`: Use Essential matrix check for epipolar inconsistency (only for Point3 landmarks).\n",
        "    - `landmarkDistanceThreshold`: If point distance is greater than this, use point-at-infinity.\n",
        "    - `dynamicOutlierRejectionThreshold`: If non-zero, dynamically rejects measurements based on reprojection error (threshold in sigmas).\n",
        "- **`retriangulationThreshold`**: If the change in camera poses between linearizations exceeds this threshold (Frobenius norm difference), the point is re-triangulated.\n",
        "- **`throwCheirality` / `verboseCheirality`**: Flags inherited from projection factors to control exception handling when a point is behind a camera."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartFactorParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import SmartProjectionParams, LinearizationMode, DegeneracyMode"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating and Modifying Params"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Default Parameters:\n",
            "linearizationMode: 0\n",
            "   degeneracyMode: 0\n",
            "rankTolerance = 1\n",
            "enableEPI = 0\n",
            "landmarkDistanceThreshold = -1\n",
            "dynamicOutlierRejectionThreshold = -1\n",
            "useLOST = 0\n",
            "noise model\n",
            "\n",
            "\n",
            "Custom Parameters:\n",
            "linearizationMode: 2\n",
            "   degeneracyMode: 1\n",
            "rankTolerance = 1e-08\n",
            "enableEPI = 0\n",
            "landmarkDistanceThreshold = -1\n",
            "dynamicOutlierRejectionThreshold = 1\n",
            "useLOST = 0\n",
            "noise model\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Default parameters\n",
        "default_params = SmartProjectionParams()\n",
        "print(\"Default Parameters:\")\n",
        "default_params.print()\n",
        "\n",
        "# Custom parameters\n",
        "custom_params = SmartProjectionParams(\n",
        "    linMode = LinearizationMode.JACOBIAN_Q,\n",
        "    degMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n",
        "    throwCheirality = False,\n",
        "    verboseCheirality = True,\n",
        "    retriangulationTh = 1e-3\n",
        ")\n",
        "# Modify triangulation parameters directly\n",
        "custom_params.setRankTolerance(1e-8)\n",
        "custom_params.setEnableEPI(False)\n",
        "custom_params.setDynamicOutlierRejectionThreshold(3.0) # Reject points with reproj error > 3*sigma\n",
        "\n",
        "print(\"\\nCustom Parameters:\")\n",
        "custom_params.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "usage_header_md"
      },
      "source": [
        "## Usage"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "usage_desc_md"
      },
      "source": [
        "These `SmartProjectionParams` objects are passed to the constructors of smart factors, like `SmartProjectionPoseFactor`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "usage_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Smart Factor created with custom params:\n",
            "SmartProjectionPoseFactor, z = \n",
            " SmartProjectionFactor\n",
            "linearizationMode: 2\n",
            "triangulationParameters:\n",
            "rankTolerance = 1e-08\n",
            "enableEPI = 0\n",
            "landmarkDistanceThreshold = -1\n",
            "dynamicOutlierRejectionThreshold = 1\n",
            "useLOST = 0\n",
            "noise model\n",
            "\n",
            "result:\n",
            "no point, status = 1\n",
            "\n",
            "SmartFactorBase, z = \n",
            "  keys = { }\n"
          ]
        }
      ],
      "source": [
        "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n",
        "\n",
        "# Example: Using custom params with a smart factor\n",
        "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "K = Cal3_S2(500, 500, 0, 320, 240)\n",
        "\n",
        "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n",
        "print(\"Smart Factor created with custom params:\")\n",
        "smart_factor.print()"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}