Create doc ipynb for Rot2
p-zach
parent
592ec4d7de
commit
320776beac
|
|
@ -0,0 +1,434 @@
|
|||
{
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 0,
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"provenance": []
|
||||
},
|
||||
"kernelspec": {
|
||||
"name": "python3",
|
||||
"display_name": "Python 3"
|
||||
},
|
||||
"language_info": {
|
||||
"name": "python"
|
||||
}
|
||||
},
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"# Rot2"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "-3NPWeM5nKTz"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"A `gtsam.Rot2` represents rotation in 2D space. It models a 2D rotation in the Special Orthogonal Group $\\text{SO}(2)$."
|
||||
],
|
||||
"metadata": {
|
||||
"id": "zKQLwRQWvRAW"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Rot2.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "1MUA6xip5fG4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"id": "7a9Dr6c5nHi1"
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"%pip install gtsam"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"from gtsam import Rot2, Point2\n",
|
||||
"import numpy as np"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "-dp28DoR7WsD"
|
||||
},
|
||||
"execution_count": 6,
|
||||
"outputs": []
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"# Initialization and properties"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "gZRXZTrJ7mqJ"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"A `Rot2` can be initialized with no arguments, which yields the identity rotation, or it can be constructed from an angle in radians, degrees, cos-sin form, or the bearing or arctangent of a 2D point. `Rot2` uses radians to communicate angle by default."
|
||||
],
|
||||
"metadata": {
|
||||
"id": "PK-HWTDm7sU4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"# The identity rotation has theta = 0.\n",
|
||||
"identity = Rot2()\n",
|
||||
"print(\"Identities:\")\n",
|
||||
"print(identity.theta())\n",
|
||||
"print(Rot2.Identity().theta())\n",
|
||||
"\n",
|
||||
"# The constructor uses radians, so it is identical to Rot2.fromAngle(r).\n",
|
||||
"rads = Rot2(np.pi / 2)\n",
|
||||
"also_rads = Rot2.fromAngle(np.pi / 2)\n",
|
||||
"print(\"Radians:\")\n",
|
||||
"print(rads.theta())\n",
|
||||
"print(also_rads.theta())\n",
|
||||
"\n",
|
||||
"# Rot2.fromDegrees(d) constructs from degrees.\n",
|
||||
"degs = Rot2.fromDegrees(90)\n",
|
||||
"print(\"Degrees:\")\n",
|
||||
"print(degs.theta())\n",
|
||||
"\n",
|
||||
"# Rot2 can also be constructed using cosine and sine values, if you have them lying around.\n",
|
||||
"c = np.cos(np.pi / 6)\n",
|
||||
"s = np.sin(np.pi / 6)\n",
|
||||
"cs = Rot2.fromCosSin(c, s)\n",
|
||||
"print(\"Cos-Sin:\")\n",
|
||||
"print(cs.theta())\n",
|
||||
"\n",
|
||||
"# Construct with bearing to point from theta = 0.\n",
|
||||
"p = Point2(2, 2)\n",
|
||||
"bear = Rot2.relativeBearing(p)\n",
|
||||
"print(\"Bearing:\")\n",
|
||||
"print(bear.theta())\n",
|
||||
"# Or with atan2(y, x), which accomplishes the same thing.\n",
|
||||
"atan = Rot2.atan2(p[1], p[0])\n",
|
||||
"print(atan.theta())"
|
||||
],
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"base_uri": "https://localhost:8080/"
|
||||
},
|
||||
"id": "oIakIOAB9afi",
|
||||
"outputId": "c2cb005d-056f-4a4f-b5ff-2226be1ba3e7"
|
||||
},
|
||||
"execution_count": 58,
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"name": "stdout",
|
||||
"text": [
|
||||
"Identities:\n",
|
||||
"0.0\n",
|
||||
"0.0\n",
|
||||
"Radians:\n",
|
||||
"1.5707963267948966\n",
|
||||
"1.5707963267948966\n",
|
||||
"Degrees:\n",
|
||||
"1.5707963267948966\n",
|
||||
"Cos-Sin:\n",
|
||||
"0.5235987755982988\n",
|
||||
"Bearing:\n",
|
||||
"0.7853981633974483\n",
|
||||
"0.7853981633974483\n"
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"The following properties are available from the standard interface:\n",
|
||||
"- `theta()` (in radians)\n",
|
||||
"- `degrees()`\n",
|
||||
"- `c()` (the cosine value, precalculated)\n",
|
||||
"- `s()` (the sine value, precalculated)\n",
|
||||
"- `matrix()` (the 2x2 rotation matrix: $\\begin{bmatrix}\n",
|
||||
"\\cos\\theta & -\\sin\\theta \\\\\n",
|
||||
"\\sin\\theta & \\cos\\theta\n",
|
||||
"\\end{bmatrix}$)"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "rHovUXbUys5r"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"example_rot = Rot2(3 * np.pi / 4)\n",
|
||||
"\n",
|
||||
"# The default print statement includes 'theta: ' and a newline at the end.\n",
|
||||
"print(example_rot)\n",
|
||||
"\n",
|
||||
"print(f\"Radians: {example_rot.theta()}\")\n",
|
||||
"print(f\"Degrees: {example_rot.degrees()}\")\n",
|
||||
"print(f\"Cosine: {example_rot.c()}\")\n",
|
||||
"print(f\"Sine: {example_rot.s()}\")\n",
|
||||
"print(f\"Matrix:\\n{example_rot.matrix()}\")\n"
|
||||
],
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"base_uri": "https://localhost:8080/"
|
||||
},
|
||||
"id": "P5OXTjFu2DeX",
|
||||
"outputId": "70848419-c055-44bc-de11-08e8f93fe3bf"
|
||||
},
|
||||
"execution_count": 18,
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"name": "stdout",
|
||||
"text": [
|
||||
"theta: 2.35619\n",
|
||||
"\n",
|
||||
"Radians: 2.356194490192345\n",
|
||||
"Degrees: 135.0\n",
|
||||
"Cosine: -0.7071067811865475\n",
|
||||
"Sine: 0.7071067811865476\n",
|
||||
"Matrix:\n",
|
||||
"[[-0.70710678 -0.70710678]\n",
|
||||
" [ 0.70710678 -0.70710678]]\n"
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"## Basic operations"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "PpqHUDnl5rTW"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"For basic use, a `Rot2` can rotate and unrotate a point."
|
||||
],
|
||||
"metadata": {
|
||||
"id": "sa4qx58n5tG9"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"rot = Rot2.fromDegrees(45)\n",
|
||||
"p = Point2(-2, 2)\n",
|
||||
"\n",
|
||||
"# Rotate the point at (-2, 2) 45 degrees to the -x axis.\n",
|
||||
"rotated = rot.rotate(p)\n",
|
||||
"print(f\"Rotated: {rotated}\")\n",
|
||||
"# Perform the inverse rotation with unrotate()\n",
|
||||
"print(f\"Unrotated: {rot.unrotate(rotated)}\")\n",
|
||||
"# Of course, unrotating a point you didn't rotate just rotates it backwards.\n",
|
||||
"print(f\"Unrotated again: {rot.unrotate(p)}\")"
|
||||
],
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"base_uri": "https://localhost:8080/"
|
||||
},
|
||||
"id": "yaBKjGn05_-c",
|
||||
"outputId": "fb89fe09-b2b8-496d-e835-f379d197a4eb"
|
||||
},
|
||||
"execution_count": 25,
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"name": "stdout",
|
||||
"text": [
|
||||
"Rotated: [-2.82842712e+00 2.22044605e-16]\n",
|
||||
"Unrotated: [-2. 2.]\n",
|
||||
"Unrotated again: [-2.22044605e-16 2.82842712e+00]\n"
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"Also, the `equals()` function allows for comparison of two `Rot2` objects with a tolerance."
|
||||
],
|
||||
"metadata": {
|
||||
"id": "RVFCoBpW6Bvh"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"eq_rads = Rot2(np.pi / 4)\n",
|
||||
"eq_degs = Rot2.fromDegrees(45)\n",
|
||||
"\n",
|
||||
"print(eq_rads.equals(eq_degs, 1e-8))\n",
|
||||
"\n",
|
||||
"# Direct comparison does not work for Rot2.\n",
|
||||
"print(eq_rads == eq_degs)"
|
||||
],
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"base_uri": "https://localhost:8080/"
|
||||
},
|
||||
"id": "m74YbK5h6CPU",
|
||||
"outputId": "1b16695e-cdfe-4348-875f-c41d8b4a3b26"
|
||||
},
|
||||
"execution_count": 31,
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"name": "stdout",
|
||||
"text": [
|
||||
"True\n",
|
||||
"False\n"
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"## Lie group $\\text{SO}(2)$"
|
||||
],
|
||||
"metadata": {
|
||||
"id": "ko9KSZgd4bCp"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"### Group operations\n",
|
||||
"\n",
|
||||
"`Rot2` implements the group operations `inverse`, `compose`, `between` and `identity`. For more information on groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
|
||||
],
|
||||
"metadata": {
|
||||
"id": "76D2KkX241zX"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"a = Rot2(np.pi / 6)\n",
|
||||
"b = Rot2(np.pi / 3)\n",
|
||||
"\n",
|
||||
"# The inverse of a Rot2 is just the negative of its angle.\n",
|
||||
"print(\"Inverse:\")\n",
|
||||
"print(a.inverse())\n",
|
||||
"\n",
|
||||
"# The composition of two Rot2 objects is their angles added together.\n",
|
||||
"# The operator for compose is *, but make no mistake, this does not multiply the angles.\n",
|
||||
"print(\"Compose:\")\n",
|
||||
"print(a * b)\n",
|
||||
"print(a.compose(b))\n",
|
||||
"\n",
|
||||
"# Between gives the difference between the two angles.\n",
|
||||
"print(\"Between:\")\n",
|
||||
"print(a.between(b))\n",
|
||||
"\n",
|
||||
"# The identity is theta = 0, as above.\n",
|
||||
"print(\"Identity:\")\n",
|
||||
"print(Rot2.Identity())"
|
||||
],
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"base_uri": "https://localhost:8080/"
|
||||
},
|
||||
"id": "QZ_NTeXK87Wq",
|
||||
"outputId": "12af920d-8f86-473d-88ab-ee57d316cb72"
|
||||
},
|
||||
"execution_count": 57,
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"name": "stdout",
|
||||
"text": [
|
||||
"Inverse:\n",
|
||||
"theta: -0.523599\n",
|
||||
"\n",
|
||||
"Compose:\n",
|
||||
"theta: 1.5708\n",
|
||||
"\n",
|
||||
"theta: 1.5708\n",
|
||||
"\n",
|
||||
"Between:\n",
|
||||
"theta: 0.523599\n",
|
||||
"\n",
|
||||
"Identity:\n",
|
||||
"theta: 0\n",
|
||||
"\n"
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"## Lie group operations\n",
|
||||
"\n",
|
||||
"`Rot2` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
|
||||
],
|
||||
"metadata": {
|
||||
"id": "YfiYuVpL-cgq"
|
||||
}
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"source": [
|
||||
"r = Rot2(np.pi / 2)\n",
|
||||
"w = Rot2(np.pi / 4)\n",
|
||||
"v = [np.pi / 2]\n",
|
||||
"\n",
|
||||
"# The exponential map transforms a 1-dimensional vector representing an angle\n",
|
||||
"# into its Rot2 equivalent.\n",
|
||||
"print(Rot2.Expmap(v))\n",
|
||||
"# The retract function takes the exponential map of the supplied 1D vector and\n",
|
||||
"# composes it with the calling Rot2.\n",
|
||||
"print(r.retract(v))\n",
|
||||
"\n",
|
||||
"# The static log map transforms a Rot2 into its 1D vector equivalent.\n",
|
||||
"print(Rot2.Logmap(r))\n",
|
||||
"# The member log map transforms a Rot2 into its 1D vector equivalent relative to\n",
|
||||
"# the Rot2 calling the function.\n",
|
||||
"print(r.logmap(w))\n",
|
||||
"# logmap is the same as calculating the coordinate of the second Rot2 in the\n",
|
||||
"# local frame of the first, which localCoordinates (inherited from LieGroup) does.\n",
|
||||
"print(r.localCoordinates(w))\n"
|
||||
],
|
||||
"metadata": {
|
||||
"colab": {
|
||||
"base_uri": "https://localhost:8080/"
|
||||
},
|
||||
"id": "4JiDEGqG-van",
|
||||
"outputId": "56047f8d-3349-4ee6-e73c-cd2fa1efb95d"
|
||||
},
|
||||
"execution_count": 54,
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"name": "stdout",
|
||||
"text": [
|
||||
"theta: 1.5708\n",
|
||||
"\n",
|
||||
"theta: 3.14159\n",
|
||||
"\n",
|
||||
"[1.57079633]\n",
|
||||
"[-0.78539816]\n",
|
||||
"[-0.78539816]\n"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Loading…
Reference in New Issue