789 lines
25 KiB
Plaintext
789 lines
25 KiB
Plaintext
{
|
|
"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": [
|
|
"<a href=\"https://colab.research.google.com/github/p-zach/myst-test/blob/main/Pose2.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": {
|
|
"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
|
|
}
|