12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2025 from borglab/add-rot2 

Create doc ipynb for Rot2
release/4.3a0
Porter Zach 2025-02-16 19:05:14 -05:00 committed by GitHub
parent 52b11e26d5 320776beac
commit 854639b275
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 434 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

434
gtsam/geometry/doc/Rot2.ipynb Normal file
View File

@ -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"
]
}
]
}
]
}