gtsam/gtsam/slam/doc/EssentialMatrixConstraint.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# EssentialMatrixConstraint"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`EssentialMatrixConstraint` is a binary factor connecting two `Pose3` variables.\n",
        "It represents a constraint derived from a measured Essential matrix ($E$) between the two camera views corresponding to the poses.\n",
        "The Essential matrix $E$ encapsulates the relative rotation $R$ and translation direction $t$ between two *calibrated* cameras according to the epipolar constraint:\n",
        "$$ p_2^T E p_1 = 0 $$\n",
        "where $p_1$ and $p_2$ are the homogeneous coordinates of corresponding points in the *normalized (calibrated)* image planes.\n",
        "\n",
        "This factor takes the measured $E_{12}$ (from pose 1 to pose 2) and compares it to the Essential matrix predicted from the estimated poses $P_1$ and $P_2$.\n",
        "The error is computed in the 5-dimensional tangent space of the Essential matrix manifold.\n",
        "\n",
        "**Note:** This factor requires known camera calibration, as the Essential matrix operates on normalized image coordinates."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/EssentialMatrixConstraint.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import EssentialMatrixConstraint, EssentialMatrix, Pose3, Rot3, Point3, Values\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating an EssentialMatrixConstraint"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "To create the factor, provide:\n",
        "1. Keys for the two `Pose3` variables (e.g., `X(1)`, `X(2)`).\n",
        "2. The measured `gtsam.EssentialMatrix` ($E_{12}$).\n",
        "3. A 5-dimensional noise model (`gtsam.noiseModel`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "EssentialMatrixConstraint: EssentialMatrixConstraint(x1,x2)\n",
            "  measured: R:\n",
            " [\n",
            "\t0.995004, -0.0998334, 0;\n",
            "\t0.0998334, 0.995004, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "d: :1\n",
            "0\n",
            "0\n",
            "isotropic dim=5 sigma=0.01\n"
          ]
        }
      ],
      "source": [
        "# Assume we have two poses\n",
        "pose1 = Pose3(Rot3.Yaw(0.0), Point3(0, 0, 0))\n",
        "pose2 = Pose3(Rot3.Yaw(0.1), Point3(1, 0, 0))\n",
        "\n",
        "# Calculate the ground truth Essential matrix between them\n",
        "gt_E12 = EssentialMatrix.FromPose3(pose1.between(pose2))\n",
        "\n",
        "# Add some noise (conceptual, E lives on a manifold)\n",
        "# In practice, E would be estimated from image correspondences\n",
        "measured_E = gt_E12 # Use ground truth for this example\n",
        "\n",
        "# Define a noise model (5 dimensional!)\n",
        "noise_dim = 5\n",
        "E_noise = gtsam.noiseModel.Isotropic.Sigma(noise_dim, 0.01)\n",
        "\n",
        "# Create the factor\n",
        "key1 = X(1)\n",
        "key2 = X(2)\n",
        "factor = EssentialMatrixConstraint(key1, key2, measured_E, E_noise)\n",
        "\n",
        "factor.print(\"EssentialMatrixConstraint: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The `.error(values)` method computes the error vector in the 5D tangent space of the Essential matrix manifold. The error represents the difference between the measured E and the E predicted from the current pose estimates in `values`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Error vector at ground truth: 0.0\n",
            "Error vector at noisy pose: 1.4069198000486227\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "# Insert values close to ground truth\n",
        "values.insert(key1, pose1)\n",
        "values.insert(key2, pose2)\n",
        "\n",
        "error_vector = factor.error(values)\n",
        "print(f\"Error vector at ground truth: {error_vector}\")\n",
        "\n",
        "# Insert values slightly different\n",
        "noisy_pose2 = Pose3(Rot3.Yaw(0.11), Point3(1.05, 0.01, -0.01))\n",
        "values.update(key2, noisy_pose2)\n",
        "\n",
        "error_vector_noisy = factor.error(values)\n",
        "print(f\"Error vector at noisy pose: {error_vector_noisy}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "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.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}