diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index b4b391df3e..52764a7cc8 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -1,6 +1,11 @@ name: Linux CI -on: [pull_request] +on: + pull_request: + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index e519164ec3..9defa5ab02 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -1,7 +1,11 @@ name: macOS CI -on: [pull_request] - +on: + pull_request: + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. concurrency: diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 24a7f6c90e..3a26f167ac 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -1,6 +1,11 @@ name: Python CI -on: [pull_request] +on: + pull_request: + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 5ad7039ed8..c1d321cf90 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -1,6 +1,11 @@ name: Special Cases CI -on: [pull_request] +on: + pull_request: + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 20b4a846f9..28aafb2dc9 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -1,6 +1,11 @@ name: Windows CI -on: [pull_request] +on: + pull_request: + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml new file mode 100644 index 0000000000..cf44fb6042 --- /dev/null +++ b/.github/workflows/deploy.yml @@ -0,0 +1,51 @@ +# This file was initialized with `myst init --gh-pages` + +name: MyST GitHub Pages Deploy +on: + push: + # Runs on pushes targeting the develop branch + branches: [develop] + # Only trigger redeploy if Markdown files, notebooks, or config changes + paths: + - '**.md' + - '**.ipynb' + - 'myst.yml' +env: + # `BASE_URL` determines the website is served from, including CSS & JS assets + # You may need to change this to `BASE_URL: ''` + BASE_URL: /${{ github.event.repository.name }} + +# Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages +permissions: + contents: read + pages: write + id-token: write +# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. +# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. +concurrency: + group: 'pages' + cancel-in-progress: false +jobs: + deploy: + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Setup Pages + uses: actions/configure-pages@v3 + - uses: actions/setup-node@v4 + with: + node-version: 18.x + - name: Install MyST Markdown + run: npm install -g mystmd + - name: Build HTML Assets + run: myst build --html + - name: Upload artifact + uses: actions/upload-pages-artifact@v3 + with: + path: './_build/html' + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v4 diff --git a/.github/workflows/trigger-packaging.yml b/.github/workflows/trigger-packaging.yml index 1f24db5032..6a31147bbb 100644 --- a/.github/workflows/trigger-packaging.yml +++ b/.github/workflows/trigger-packaging.yml @@ -4,6 +4,10 @@ on: push: branches: - develop + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' jobs: trigger-package-build: runs-on: ubuntu-latest diff --git a/.github/workflows/trigger-python.yml b/.github/workflows/trigger-python.yml index 1e8981d999..ab27d9deec 100644 --- a/.github/workflows/trigger-python.yml +++ b/.github/workflows/trigger-python.yml @@ -4,6 +4,10 @@ on: push: branches: - develop + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' jobs: triggerPython: runs-on: ubuntu-latest diff --git a/.gitignore b/.gitignore index 6c8ac97b54..7496190b54 100644 --- a/.gitignore +++ b/.gitignore @@ -20,3 +20,6 @@ xcode/ /Dockerfile /python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb .cache/ + +# MyST build outputs +_build diff --git a/INSTALL.md b/INSTALL.md index 3ebbb3c6aa..dd79a33f9b 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -1,4 +1,6 @@ -# Quickstart +# Installation Guide + +## Quickstart In the root library folder execute: diff --git a/README.md b/README.md index e02aa953c4..e30062e2e7 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# README - Georgia Tech Smoothing and Mapping Library +# GTSAM: Georgia Tech Smoothing and Mapping Library **Important Note** diff --git a/gtsam/geometry/doc/Pose2.ipynb b/gtsam/geometry/doc/Pose2.ipynb new file mode 100644 index 0000000000..660c78ac62 --- /dev/null +++ b/gtsam/geometry/doc/Pose2.ipynb @@ -0,0 +1,788 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "xhCSxfYqLPVI" + }, + "source": [ + "# Pose2" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "cF2tPRNbLSIJ" + }, + "source": [ + "A `Pose2` represents a position and orientation in 2D space. It is both a $\\mathcal{SE}(2)$ pose manifold and a $SE(2)$ Lie group of transforms. It consists of a 2D position (variously represented as $r$, $t$, or $(x, y)$ depending on the context) and a rotation (similarly, $C$, $R$, or $\\theta$). Its 3-dimensional analog is a `Pose3`. It is included in the top-level `gtsam` package." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "OJNfomRj5H-C" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "DZrO0rl2LfIB", + "outputId": "162d0ca4-9bb2-4407-b317-052c20327110" + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": { + "id": "FJKBejrlLdrv" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import Pose2, Point2, Rot2, Point3\n", + "import numpy as np" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "KacaiUrnLXzR" + }, + "source": [ + "## Initialization and properties\n", + "\n", + "A `Pose2` can be initialized with no arguments, which yields the identity pose, or it can be constructed with a position and rotation." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "Ej6_SXQMLcH5", + "outputId": "b889f078-4feb-4a1a-9a48-b4becfd0aede" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(0, 0, 0)\n", + "\n", + "(1, 2, 1.5708)\n", + "\n" + ] + } + ], + "source": [ + "# Identity pose\n", + "p1 = Pose2()\n", + "print(p1)\n", + "\n", + "R = Rot2.fromDegrees(90)\n", + "# Point2 is not an object, it is a utility function that creates a 2d float np.ndarray\n", + "t = Point2(1, 2) # or (1, 2) or [1, 2] or np.array([1, 2])\n", + "# Pose at (1, 2) and facing north\n", + "p2 = Pose2(R, t)\n", + "print(p2)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "hec_a8HHV4Zf" + }, + "source": [ + "The pose's properties can be accessed using the following members:\n", + "- `x()`\n", + "- `y()`\n", + "- `translation()` (which returns a two-element `np.ndarray` representing `x, y`)\n", + "- `theta()`\n", + "- `rotation()` (which returns a [`Rot2`](./Rot2.ipynb))\n", + "- `matrix()`\n", + "\n", + "The `matrix()` function returns the pose's rotation and translation in the following form:\n", + "\n", + "$$\n", + "T =\n", + "\\begin{bmatrix}\n", + "R & t \\\\\n", + "0 & 1\n", + "\\end{bmatrix}\n", + "=\n", + "\\begin{bmatrix}\n", + "\\cos\\theta & -\\sin\\theta & x \\\\\n", + "\\sin\\theta & \\cos\\theta & y \\\\\n", + "0 & 0 & 1\n", + "\\end{bmatrix}\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "Lcj3o7kIV0Ez", + "outputId": "881ecfe6-f7f4-42ba-e262-f8e8694bb10b" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Location: (1.0, 2.0); also accessible with translation(): [1. 2.]\n", + "Rotation: 1.5707963267948966; also accessible with rotation(): 1.5707963267948966\n", + "Position-rotation 3x3 matrix:\n", + "[[ 6.123234e-17 -1.000000e+00 1.000000e+00]\n", + " [ 1.000000e+00 6.123234e-17 2.000000e+00]\n", + " [ 0.000000e+00 0.000000e+00 1.000000e+00]]\n" + ] + } + ], + "source": [ + "print(f\"Location: ({p2.x()}, {p2.y()}); also accessible with translation(): {p2.translation()}\")\n", + "\n", + "# .rotation() returns a Rot2 object; the float value can be accessed with .theta()\n", + "print(f\"Rotation: {p2.theta()}; also accessible with rotation(): {p2.rotation().theta()}\")\n", + "\n", + "print(f\"Position-rotation 3x3 matrix:\\n{p2.matrix()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "iW1E7uMZLjhc" + }, + "source": [ + "## Basic operations\n", + "Points in the global space can be transformed to and from the local space of the `Pose2` using `transformTo` and `transformFrom`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "bHh9tce9LLVN", + "outputId": "dafd6946-5bc3-4620-c414-f8302a5823de" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[5. 5.] transformed by (1, 1, 3.14159)\n", + " into local space -> [-4. -4.]\n", + "[-4. -4.] transformed by (1, 1, 3.14159)\n", + " into global space -> [5. 5.]\n" + ] + } + ], + "source": [ + "global_point = Point2(5, 5)\n", + "origin = Pose2(Rot2.fromAngle(np.pi), Point2(1, 1))\n", + "\n", + "# For a point at (1, 1) that is rotated 180 degrees, a point at (5, 5) in global\n", + "# space is at (-4, -4) in local space.\n", + "transformed = origin.transformTo(global_point)\n", + "print(f\"{global_point} transformed by {origin} into local space -> {transformed}\")\n", + "\n", + "reversed = origin.transformFrom(transformed)\n", + "print(f\"{transformed} transformed by {origin} into global space -> {reversed}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "HDutn5zTLnv9" + }, + "source": [ + "Bearings (angles) and ranges (distances) can be calculated to points using the associated functions `bearing` and `range`." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "t_KrFGybLpY1", + "outputId": "c3d52977-20b2-4675-eaa1-3a93071a0066" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Bearing from (-3, -3, 1.5708)\n", + " to [-2. -3.]: -1.5707963267948966\n", + "Bearing from (1, 1, -0.785398)\n", + " to [0. 2.]: 3.141592653589793\n", + "Range from (4, 0, -1.5708)\n", + " to [0. 3.]: 5.0\n" + ] + } + ], + "source": [ + "p1 = Pose2(Rot2.fromDegrees(90), Point2(-3, -3))\n", + "point1 = Point2(-2, -3)\n", + "print(f\"Bearing from {p1} to {point1}: {p1.bearing(point1).theta()}\")\n", + "\n", + "p2 = Pose2(Rot2.fromDegrees(-45), Point2(1, 1))\n", + "p3 = Pose2(Rot2.fromDegrees(180), Point2(0, 2))\n", + "print(f\"Bearing from {p2} to {p3.translation()}: {p2.bearing(p3.translation()).theta()}\")\n", + "\n", + "p4 = Pose2(Rot2.fromDegrees(-90), Point2(4, 0))\n", + "point2 = Point2(0, 3)\n", + "print(f\"Range from {p4} to {point2}: {p4.range(point2)}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "z5C0ARZ3D3SP" + }, + "source": [ + "## Manifold $\\mathcal{SE}(2)$\n", + "\n", + "The manifold $\\mathcal{SE}(2)$ represents poses in 2D space and is defined by elements $(C, r)$ where:\n", + "- $C \\in \\mathcal{SO}(2)$: The orientation or attitude.\n", + "- $r \\in \\mathbb{R}^2$: The position.\n", + "\n", + "This manifold can be thought of as all possible values that can specify the position and orientation of a rigid body in some reference frame. It does not have a notion of composition; we would never think to *compose* two poses of two different rigid bodies.\n", + "\n", + "### Manifold operations\n", + "\n", + "#### Local\n", + "\n", + "The $\\text{local}(p, q)$ function maps $q$ into the local coordinate system of $p$. Essentially, it \"subtracts\" $p$ from $q$ on the manifold, giving the relative pose in the local frame of $p$. In other words, it computes the difference between two points on a manifold and expresses the result in the tangent space. It is a form of the Lie group operation $p^{-1}q$.\n", + "\n", + "Mathematically, the local function is computed as follows:\n", + "\n", + "$$\n", + "\\text{local}(p,q) = (C_p^TC_q, C_p^T(r_q-r_p))\n", + "$$\n", + "\n", + "where:\n", + "- $p = (C_p, r_p)$\n", + "- $q = (C_q, r_q)$\n", + "\n", + "so:\n", + "- $C_p^TC_q$ is the relative rotation.\n", + "- $C_p^T(r_q-r_p)$ is the relative translation in the frame of $p$.\n", + "\n", + "\n", + "Call the `localCoordinates` member function to use the local function in your code. In GTSAM, the result of the local function is interpreted as a vector in the tangent space, so `localCoordinates` returns a 3-element `np.ndarray`." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "oIL0qdCophQL", + "outputId": "c3bb33e2-1123-4865-dce6-6a38caa44dbf" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[ 7. -6. -2.35619449]\n" + ] + } + ], + "source": [ + "p = Pose2(Rot2.fromDegrees(90), Point2(-5, -3))\n", + "q = Pose2(Rot2.fromDegrees(-45), Point2(1, 4))\n", + "\n", + "print(p.localCoordinates(q))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "HGxaHWLY29h4" + }, + "source": [ + "\n", + "#### Retract\n", + "\n", + "The $\\text{retract}(p, v)$ function takes a point $p$ on the manifold and a perturbation $v$ from the tangent space and maps it back onto the manifold to get a new point. It is the inverse of the $\\text{local}$ function, so it may be considered as $p + v$.\n", + "\n", + "Given:\n", + "- An initial pose $p = (x, y, \\theta)$\n", + "- A perturbation $v = (v_x, v_y, v_\\theta)$\n", + "\n", + "The retract function updates the pose using the following formula:\n", + "\n", + "$$\n", + "\\text{Retract}(T, v) =\n", + "\\begin{cases}\n", + "\\begin{bmatrix} x \\\\ y \\end{bmatrix} +\n", + "R(\\theta) \\begin{bmatrix} i \\\\ j \\end{bmatrix}, \\quad \\theta + v_\\theta, & \\text{if } v_\\theta \\neq 0 \\\\\n", + "\\begin{bmatrix} x + v_x \\\\ y + v_y \\end{bmatrix}, \\quad \\theta + v_\\theta, & \\text{if } v_\\theta \\approx 0\n", + "\\end{cases}\n", + "$$\n", + "\n", + "where:\n", + "- $R(\\theta)$ is the rotation matrix:\n", + " $$\n", + " R(\\theta) = \\begin{bmatrix} \\cos\\theta & -\\sin\\theta \\\\ \\sin\\theta & \\cos\\theta \\end{bmatrix}\n", + " $$\n", + "- $i, j$ are computed based on the motion model:\n", + " $$\n", + " i = \\frac{v_x \\sin v_\\theta + v_y (1 - \\cos v_\\theta)}{v_\\theta}\n", + " $$\n", + " $$\n", + " j = \\frac{v_x (1 - \\cos v_\\theta) + v_y \\sin v_\\theta}{v_\\theta}\n", + " $$\n", + "\n", + "Call the `retract` member function to use the retract function in your code. Since $v$ is in the tangent space, it must be passed as a 3D vector (a 3-element `np.ndarray`). `retract` returns the adjusted $p$." + ] + }, + { + "cell_type": "code", + "execution_count": 27, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "xen8-BTfs1Nw", + "outputId": "59a22c33-3226-4cd8-f4d0-d28f582e7d51" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(-4, -1, -1.5708)\n", + "\n", + "(1, 4, -0.785398)\n", + "\n", + "(1, 4, -0.785398)\n", + "\n" + ] + } + ], + "source": [ + "p = Pose2(Rot2.fromDegrees(90), Point2(-5, -3))\n", + "v = Point3(2, -1, Rot2.fromDegrees(180).theta())\n", + "q = Pose2(Rot2.fromDegrees(-45), Point2(1, 4))\n", + "\n", + "print(p.retract(v))\n", + "\n", + "print(q)\n", + "# Applies local and then retract, which cancel out. This statement prints q given\n", + "# any p.\n", + "print(p.retract(p.localCoordinates(q)))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "f3kbsmcn0Dgz" + }, + "source": [ + "### Optimization on $\\mathcal{SE}(2)$\n", + "\n", + "Many problems involve optimization over the pose manifold, such as:\n", + "\n", + "- **Localization**: Estimating the pose of a robot or sensor in a global or local frame.\n", + "- **SLAM (Simultaneous Localization and Mapping)**: Optimizing a graph of poses and landmarks to minimize error.\n", + "\n", + "Examples of these problems can be found at the end of this page." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "y90I_MSV6y6G" + }, + "source": [ + "## Lie group $SE(2)$\n", + "\n", + "### Group operations\n", + "\n", + "A `Pose2` implements the group operations `identity`, `inverse`, `compose`, and `between`. For more information on groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html).\n", + "\n", + "#### Identity\n", + "\n", + "The `Pose2` identity is $(0, 0, 0)$." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "hNcTx2Uq0syd", + "outputId": "e672ddff-8fed-405f-c5a0-1aea69f0bf3f" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(0, 0, 0)\n", + "\n" + ] + } + ], + "source": [ + "print(Pose2.Identity())" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "hNZFb84H0tOh" + }, + "source": [ + "#### Inverse\n", + "\n", + "The inverse of a pose represents the transformation that undoes the pose. In other words, if you have a pose $T$ that moves from frame A to frame B, its inverse $T^{-1}$ moves from frame B back to frame A. The equation to compute the inverse is as follows:\n", + "\n", + "$$\n", + "T^{-1} = (-x \\cos\\theta - y \\sin\\theta, x \\sin\\theta - y \\cos\\theta, -\\theta)\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "jhw27T7E0t9b", + "outputId": "1c5a47e9-2484-41f1-83b0-0380fa5f86b0" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Inverse of (-5, 2, 0)\n", + " -> (5, -2, -0)\n", + "\n", + "Inverse of (6, 4, 0.785398)\n", + " -> (-7.07107, 1.41421, -0.785398)\n", + "\n" + ] + } + ], + "source": [ + "p5 = Pose2(0, Point2(-5, 2))\n", + "print(f\"Inverse of {p5} -> {p5.inverse()}\")\n", + "\n", + "p6 = Pose2(Rot2.fromDegrees(45), Point2(6, 4))\n", + "print(f\"Inverse of {p6} -> {p6.inverse()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Bic1WsVu0uO4" + }, + "source": [ + "#### Composition\n", + "\n", + "The composition of two `Pose2` objects follows the rules of $\\mathcal{SE}(2)$ transformation.\n", + "\n", + "Given two poses:\n", + "- Pose A: $T_A = (x_A, y_A, \\theta_A)$\n", + "- Pose B: $T_B = (x_B, y_B, \\theta_B)$\n", + "\n", + "The composition of these two poses $T_C = T_A \\cdot T_B$ results in:\n", + "\n", + "$$\n", + "x_C = x_A + \\cos(\\theta_A) x_B - \\sin(\\theta_A) y_B\n", + "$$\n", + "$$\n", + "y_C = y_A + \\sin(\\theta_A) x_B + \\cos(\\theta_A) y_B\n", + "$$\n", + "$$\n", + "\\theta_C = \\theta_A + \\theta_B\n", + "$$\n", + "\n", + "Therefore:\n", + "\n", + "$$\n", + "T_C = ( x_A + \\cos(\\theta_A) x_B - \\sin(\\theta_A) y_B,\\space y_A + \\sin(\\theta_A) x_B + \\cos(\\theta_A) y_B,\\space \\theta_A + \\theta_B )\n", + "$$\n", + "\n", + "In other words:\n", + "- The rotation of Pose A is applied to the translation of Pose B before adding it.\n", + "- The final rotation is just the sum of the two rotations." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "i_dEwknx7xMb", + "outputId": "b979f426-fed3-49f4-942b-341d34fdf465" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Composition: (8, 10, 0)\n", + " * (4, -7, 2.35619)\n", + " -> (12, 3, 2.35619)\n", + "\n", + "Composition is not commutative: (4, -7, 2.35619)\n", + " * (8, 10, 0)\n", + " = (-8.72792, -8.41421, 2.35619)\n", + "\n" + ] + } + ], + "source": [ + "p7 = Pose2(0, Point2(8, 10))\n", + "p8 = Pose2(Rot2.fromDegrees(135), Point2(4, -7))\n", + "\n", + "print(f\"Composition: {p7} * {p8} -> {p7 * p8}\")\n", + "print(f\"Composition is not commutative: {p8} * {p7} = {p8 * p7}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fr_nFfqk37pu" + }, + "source": [ + "#### Between\n", + "\n", + "Given two poses $T_A$ and $T_B$, the between function returns the pose of $T_B$ in the local coordinate frame of $A$; in other words, the transformation needed to move from $T_A$ to $T_B$.\n", + "\n", + "The between function is given by:\n", + "\n", + "$$\n", + "T_{T_A→T_B} = T_A^{-1} \\cdot T_B\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": 44, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "AMPoJLJB46K6", + "outputId": "a5882ad6-da09-41e2-9333-7f047a65e0fa" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(-4, -4, 0.785398)\n", + "\n" + ] + } + ], + "source": [ + "a = Pose2(Rot2.fromDegrees(0), Point2(1, 4))\n", + "b = Pose2(Rot2.fromDegrees(45), Point2(-3, 0))\n", + "\n", + "print(a.between(b))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "yyFARpm8Ag9u" + }, + "source": [ + "### Lie group operations\n", + "\n", + "A `Pose2` also implements the Lie group operations for exponential mapping, log mapping, and adjoint mapping, as well as other Lie group functionalities. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html).\n", + "\n", + "#### Exponential mapping\n", + "\n", + "The exponential map function `expmap` is used to convert a small motion, like a velocity or perturbation, in the Lie algebra (tangent space) into a `Pose2` transformation in the Lie group $\\mathcal{SE}(2)$. It is used because optimization is easier in the tangent space; transformations behave like vectors there.\n", + "\n", + "In tangent space, small motions are represented as:\n", + "\n", + "$$\n", + "\\xi = (\\nu_x, \\nu_y, \\omega)\n", + "$$\n", + "\n", + "where:\n", + "- $\\nu_x, \\nu_y$ are small translations in the local frame.\n", + "- $\\omega$ is a small rotation.\n", + "\n", + "The exponential map converts this small motion into a full pose:\n", + "\n", + "$$\n", + "T = \\exp(\\xi) = \\begin{cases}\n", + " (x, y, \\theta) = (\\nu_x, \\nu_y, \\omega) & \\text{if } \\omega = 0 \\\\\n", + " \\left( \\frac{\\sin\\omega}{\\omega} \\nu_x - \\frac{1 - \\cos\\omega}{\\omega} \\nu_y, \\frac{1 - \\cos\\omega}{\\omega} \\nu_x + \\frac{\\sin\\omega}{\\omega} \\nu_y, \\omega \\right) & \\text{otherwise}\n", + "\\end{cases}\n", + "$$\n", + "\n", + "This accounts for rotational effects when mapping from the tangent space back to $\\mathcal{SE}(2)$." + ] + }, + { + "cell_type": "code", + "execution_count": 45, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "hzsXmrzdAorc", + "outputId": "4d2bcb94-8a98-43e1-f8d0-234c6c3d6ed6" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(0, 0.63662, 1.5708)\n", + "\n" + ] + } + ], + "source": [ + "twist = gtsam.Point3(0.5, 0.5, Rot2.fromDegrees(90).theta())\n", + "\n", + "print(Pose2.Expmap(twist))\n", + "# There is no pose.expmap(...), only Pose2.Expmap(...). If you are looking to\n", + "# convert a small motion to a pose relative to a pose, use retract." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "8n0ELPxLzCSs" + }, + "source": [ + "#### Log mapping\n", + "\n", + "The log map function `logmap` is used to convert a transformation in $\\mathcal{SE}(2)$ (such as a `Pose2`) into a vector in tangent space. It can be used to convert a pose to its small motion representation or compute the difference between two poses.\n", + "\n", + "For a pose $T = (x,y,\\theta)$, `logmap` finds the equivalent motion in tangent space:\n", + "\n", + "$$\n", + "\\log(T) = \\left( \\begin{array}{c} V^{-1} \\cdot t \\\\ \\theta \\\\ \\end{array} \\right) = \\xi = (\\nu_x, \\nu_y, \\omega)\n", + "$$\n", + "\n", + "where:\n", + "\n", + "- $V^{-1} = \\frac{1}{A^2+B^2} \\left( \\begin{array}{cc} A & B \\\\ -B & A \\end{array} \\right)$\n", + "- $A = \\frac{\\sin(\\theta)}{\\theta}$\n", + "- $B = \\frac{1 - \\cos(\\theta)}{\\theta}$\n", + "- $t = (x, y)$" + ] + }, + { + "cell_type": "code", + "execution_count": 49, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "AlWLKMSWzDAM", + "outputId": "2d5d5c48-f17e-42f7-b969-4e5a07e33bed" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[-6.29474529 -8.12827598 2.35619449]\n", + "[0. 0. 0.]\n", + "[-1.41421356 -1.41421356 0. ]\n" + ] + } + ], + "source": [ + "pose = Pose2(Rot2.fromDegrees(135), Point2(4, -7))\n", + "diff = Pose2(Rot2.fromDegrees(135), Point2(6, -7))\n", + "\n", + "# Convert a pose to its small motion representation\n", + "print(Pose2.Logmap(pose))\n", + "\n", + "# Compute the difference between two poses\n", + "print(pose.logmap(pose))\n", + "print(pose.logmap(diff))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "oW3mEWvixYr8" + }, + "source": [ + "## Advanced concepts" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "rpa_0pUBxdoo" + }, + "source": [ + "### Adjoint mapping\n", + "\n", + "### Jacobians" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Oka0IPN2Lp29" + }, + "source": [ + "## Example: SLAM\n", + "`Pose2` can be used as the basis to perform simultaneous localization and mapping (SLAM), as seen in the example [here](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/Pose2SLAMExample.py).\n" + ] + } + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/geometry/doc/Pose3.ipynb b/gtsam/geometry/doc/Pose3.ipynb new file mode 100644 index 0000000000..25a45fafb5 --- /dev/null +++ b/gtsam/geometry/doc/Pose3.ipynb @@ -0,0 +1,395 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Pose3\n", + "\n", + "The `Pose3` class implements two things: a pose manifold and a Lie group of transforms. They look awfully similar, and are often treated as the same thing, but they are really not." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "vscode": { + "languageId": "html" + } + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [], + "source": [ + "%pip install gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": { + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Rot3, Point3" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Manifold $\\mathcal{SE}(3)$\n", + "The manifold $\\mathcal{SE}(3)$ is defined by elements $(C, r)$, where:\n", + "- $C \\in \\mathcal{SO}(3)$: The orientation or **attitude**.\n", + "- $r \\in \\mathbb{R}^3$: The **position**.\n", + "\n", + "This manifold represents poses in 3D space, with $C$ encoding orientation/attitude and $r$ encoding the position of a body in some reference frame. \n", + "\n", + "This manifold can be thought of all possible values that can specify the position and orientation of a rigid body in some reference frame. It does not have a notion of composition: we would never think to *compose* two poses of two different rigid bodies." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The simplest thing you can do in GTSAM is just creating a pose with identity orientation and position:" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "P=\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0 0 0\n", + "\n" + ] + } + ], + "source": [ + "P = Pose3()\n", + "print(f\"P=\\n{P}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "But of course, it is more useful to specify some orientation and position. To do that, you need to create a `Rot3` instance, see [Rotations in 3D](Rot3.ipynb). For example, here is a pose of a robot at $r=(3,4,0)$ and rotated by 30 degrees around the Z-axis:" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": { + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "R: [\n", + "\t0.866025, -0.5, 0;\n", + "\t0.5, 0.866025, 0;\n", + "\t0, 0, 1\n", + "]\n", + " [3. 4. 0.]\n", + "P=\n", + "R: [\n", + "\t0.866025, -0.5, 0;\n", + "\t0.5, 0.866025, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 3 4 0\n", + "\n" + ] + } + ], + "source": [ + "C = Rot3.Yaw(np.deg2rad(30))\n", + "r = Point3(3,4,0)\n", + "print(C,r)\n", + "P = Pose3(C,r)\n", + "print(f\"P=\\n{P}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A great many things can be done using just the manifold $\\mathcal{SE}(3)$, as it provides a flexible and foundational framework for representing and reasoning about poses in 3D space. Below are some key use cases and examples:\n", + "\n", + "### Trajectories on $\\mathcal{SE}(3)$\n", + "A trajectory over time can be represented as a continuous or discrete curve on the manifold:\n", + "$$\n", + "P(t) = (C(t), r(t)),\n", + "$$\n", + "where:\n", + "- $C(t)$ is a time-varying orientation.\n", + "- $r(t)$ is a time-varying position vector.\n", + "\n", + "This representation is useful for describing motion paths of rigid bodies, such as robots, drones, or vehicles.\n", + "### Optimization on $\\mathcal{SE}(3)$\n", + "Many problems involve optimization over the pose manifold, such as:\n", + "- **Localization**: Estimating the pose of a robot or sensor in a global or local frame.\n", + "- **SLAM (Simultaneous Localization and Mapping)**: Optimizing a graph of poses and landmarks to minimize error." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Lie Group $SE(3)$\n", + "The Lie group $SE(3)$ consists of elements $(R, t)$, where:\n", + "- $R \\in SO(3)$: A **rotation** matrix.\n", + "- $t \\in \\mathbb{R}^3$: A **translation** vector.\n", + "\n", + "The group operation in $SE(3)$ is defined as:\n", + "$$\n", + "(R_1, t_1) \\cdot (R_2, t_2) = (R_1 R_2, R_1 t_2 + t_1).\n", + "$$\n", + "This operation combines two transformations, where $R_1 R_2$ composes the rotations and $R_1 t_2 + t_1$ composes the translations.\n", + "\n", + "> Note that this particular form of composition *defines* $SE(3)$ as the *semi-product* of the group $SO(3)$ and the vector space $\\mathbb{R}^3$. This should be contrasted with a direct product, which would yield $(R_1 R_2, t_2 + t_1)$ as the composition.\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Group Action on 3D Points\n", + "The group $SE(3)$ acts on a 3D point $p \\in \\mathbb{R}^3$ as follows:\n", + "$$\n", + "p' = (R, t) \\cdot p = R p + t.\n", + "$$\n", + "Here, the rotation $R$ is applied first, followed by the translation $t$." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "### Group Action on Poses\n", + "The group $SE(3)$ also acts on a pose $(C,r) \\in \\mathcal{SE}(3)$ as follows:\n", + "$$\n", + "(R,t) \\cdot (C,r) = (R C, R r +t).\n", + "$$\n", + "- $R C$ rotates the orientation into a different frame.\n", + "- $R r +t$ transforms the position, by first rotating the position in the old frame, then translating to account for the origin $t$ of the new frame." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 4x4 Matrix Lie Group View of Transforms\n", + "\n", + "A transform in $SE(3)$ can also be represented as a $4 \\times 4$ homogeneous transformation matrix:\n", + "$$\n", + "T =\n", + "\\begin{bmatrix}\n", + "R & t \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix},\n", + "$$\n", + "\n", + "The composition of two transforms $T_1$ and $T_2$ in $SE(3)$ is then simply matrix multiplication\n", + "$$\n", + "T_1 \\cdot T_2 =\n", + "\\begin{bmatrix}\n", + "R_1 & t_1 \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "R_2 & t_2 \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix}\n", + "=\n", + "\\begin{bmatrix}\n", + "R_1 R_2 & R_1 t_2 + t_1 \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix}\n", + "$$\n", + "\n", + "and the inverse is given by:\n", + "$$\n", + "T^{-1} =\n", + "\\begin{bmatrix}\n", + "R^\\top & -R^\\top t \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix},\n", + "$$\n", + "where $R^\\top$ is the transpose (and also the inverse) of the rotation matrix $R$.\n", + "\n", + "To implement the action of $SE(3)$ on $\\mathbb{R}^3$ we need to embed a point $p$ in homogeneous coordinates as $\\tilde{p} = [p^\\top, 1]^\\top$, where $p \\in \\mathbb{R}^3$. The action of a transform $T$ on a point $\\tilde{p}$ is then given by:\n", + "$$\n", + "\\tilde{p}' = T \\tilde{p} =\n", + "\\begin{bmatrix}\n", + "R & t \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "p \\\\\n", + "1\n", + "\\end{bmatrix}\n", + "=\n", + "\\begin{bmatrix}\n", + "R p + t \\\\\n", + "1\n", + "\\end{bmatrix} = \\begin{bmatrix}\n", + "p' \\\\\n", + "1\n", + "\\end{bmatrix}.\n", + "$$\n", + "\n", + "The matrix representation provides an elegant way to handle transformations in 3D space. This view also emphasizes that $SE(3)$ is a **matrix Lie group**, i.e., a subgroup of invertible $d\\times d$ matrices, with in this case $d=4$. However, it is by no means required to implement the Lie group $SE(3)$ and in fact GTSAM internally does *not* use $4\\times 4$ matrices." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Reference Frame Hierarchies\n", + "\n", + "Poses can sometimes be used as transforms when hierarchical relationships exist between frames. For example, consider the pose $P^b_i$ of an IMU relative to a rigid body B, which itself has a pose $P^n_b$ in a navigation frame N.\n", + "\n", + "If we seek is the pose $P^n_i$ of the IMU relative to the navigation frame, we need to *upgrade* the body pose $P^n_b$ to a rigid transform $T^n_b$. The pose of the IMU relative to the navigation frame is then given by:\n", + "$$\n", + "P^n_i = T^n_b \\cdot P^b_i.\n", + "$$\n", + "Note how the $b$ indices cancel out." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Retractions and the Exponential Map\n", + "\n", + "### The Tangent Space of $SE(3)$\n", + "\n", + "The tangent space of $SE(3)$ at the identity, known as its **Lie algebra** $\\mathfrak{se}(3)$, can be thought of informally as encoding **angular and linear velocities**. Elements of $\\mathfrak{se}(3)$ are twists $\\xi = (\\omega, v)$, where:\n", + "- $\\omega \\in \\mathbb{R}^3$: Represents angular velocity.\n", + "- $v \\in \\mathbb{R}^3$: Represents linear velocity.\n", + "\n", + "These quantities describe instantaneous motion in 3D space. A twist $\\xi$ can be represented as a skew-symmetric matrix:\n", + "$$\n", + "\\xi^\\wedge =\n", + "\\begin{bmatrix}\n", + "\\omega^\\wedge & v \\\\\n", + "0^\\top & 0\n", + "\\end{bmatrix},\n", + "$$\n", + "where $\\omega^\\wedge$ is the skew-symmetric matrix of $\\omega$, encoding rotational motion.\n", + "\n", + "To describe **finite motions**, the twist $\\xi$ is scaled by a time or displacement parameter $\\Delta t$, resulting in a scaled twist $\\xi \\Delta t = (\\phi, \\rho)$:\n", + "- $\\phi = \\omega \\Delta t$: A finite angular displacement.\n", + "- $\\rho = v \\Delta t$: A finite linear displacement.\n", + "\n", + "We used $\\Delta t$ to indicate a *time* interval: not this has nothing to do with the *translation* $t$. This finite motion $\\xi \\Delta t$ is the input to the **exponential map**, which we explain next." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### The Exponential Map for $SE(3)$\n", + "\n", + "The **exponential map** for $SE(3)$ takes a finite motion $\\xi \\Delta t = (\\phi, \\rho)$, derived from a twist $\\xi = (\\omega, v)$ scaled by a parameter $t$, and maps it to a rigid body transform $(R, t) \\in SE(3)$. \n", + "The exponential map produces a transformation $(R, t)$ in $SE(3)$:\n", + "$$\n", + "\\exp((\\xi \\Delta t)^\\wedge) =\n", + "\\begin{bmatrix}\n", + "R & t \\\\\n", + "0^\\top & 1\n", + "\\end{bmatrix},\n", + "$$\n", + "or $(R,t)\\in SE(3)$, where\n", + "- $R = \\exp(\\phi^\\wedge)$, where $\\phi^\\wedge$ is the skew-symmetric matrix of $\\phi$.\n", + "- $t = J_l(\\phi) \\rho$, where $J_l(\\phi)$ is the *left Jacobian* of $SO(3)$, a $3 \\times 3$ matrix. $J_l(\\phi)$ handles the semi-direct product nature of $SE(3)$ by coupling the rotational and translational components appropriately.\n", + "\n", + "An often-used shorthand notation is defining $\\text{Exp}: \\mathbb{R}^6 \\rightarrow SE(3)$ or a two-argument variant as:\n", + "$$\n", + "\\text{Exp}(\\phi,\\rho) = \\text{Exp}(\\xi \\Delta t) \\doteq \\exp((\\xi \\Delta t)^\\wedge) = (\\text{Exp}(\\phi), J_l(\\phi) \\rho)\n", + "$$\n", + "where we used the same notation for $SO(3)$, i.e., $\\text{Exp}(\\phi)\\doteq \\exp(\\phi^\\wedge)$.\n", + "\n", + "### Retractions to the Manifold $\\mathcal{SE}(3)$\n", + "\n", + "The exponential map can also be used as a **retraction** for optimization on the $SE(3)$ manifold. Retractions are mappings from the tangent space to the manifold, e.g., the exponential map implements the following retraction:\n", + "$$\n", + "(C, r) \\times (\\phi, \\rho) \\mapsto \\exp((\\xi \\Delta t)^\\wedge) \\cdot (C,r) = (C \\exp(\\phi^\\wedge), r + C J_l(\\phi) \\rho),\n", + "$$\n", + "\n", + "Alternative retractions exists, however, and the only restriction on them is that they need to have the same behavior as the exponential map near the identity. One alternative retraction simply treats the attitude and position separately:\n", + "$$\n", + "(C, r) \\times (\\phi, \\rho) \\mapsto (C\\exp(\\phi^\\wedge), r + \\rho),\n", + "$$\n", + "This alternative retraction behaves identically to the exponential map at $t = 0$ and is computationally simpler." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtbook", + "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.9.19" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/myst.yml b/myst.yml new file mode 100644 index 0000000000..2ada9c7f78 --- /dev/null +++ b/myst.yml @@ -0,0 +1,31 @@ +# See docs at: https://mystmd.org/guide/frontmatter +version: 1 +project: + id: 7a62a86d-a893-4ab1-9473-b1a957f78902 + title: GTSAM Docs + description: GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices. + github: https://github.com/borglab/gtsam + toc: + - file: README.md + - file: INSTALL.md + - title: User Guide + children: + - title: Geometry + children: + - pattern: './gtsam/geometry/doc/*' +site: + nav: + - title: Getting started + children: + - title: Overview + url: /readme + - title: Install guide + url: /install + - title: C++ reference + url: https://gtsam.org/doxygen/ + template: book-theme + + # TODO: Graphics for favicon and to replace "Made with MyST" in the top left + # options: + # favicon: favicon.ico + # logo: site_logo.png