diff --git a/gtsam/geometry/doc/Rot2.ipynb b/gtsam/geometry/doc/Rot2.ipynb new file mode 100644 index 0000000000..704915b4c6 --- /dev/null +++ b/gtsam/geometry/doc/Rot2.ipynb @@ -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": [ + "\"Open" + ], + "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" + ] + } + ] + } + ] +} \ No newline at end of file