From 515127cbe90148e62b660f7e4ae7e679e4182272 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 17 Apr 2025 10:28:41 -0400 Subject: [PATCH] %pip install --quiet gtsam-develop --- gtsam/geometry/doc/Pose2.ipynb | 2 +- gtsam/geometry/doc/Pose3.ipynb | 2 +- gtsam/geometry/doc/Rot2.ipynb | 408 +++++----- gtsam/geometry/doc/Rot3.ipynb | 896 +++++++++++----------- gtsam/inference/doc/BayesNet.ipynb | 2 +- gtsam/inference/doc/BayesTree.ipynb | 2 +- gtsam/inference/doc/Conditional.ipynb | 2 +- gtsam/inference/doc/DotWriter.ipynb | 2 +- gtsam/inference/doc/EdgeKey.ipynb | 2 +- gtsam/inference/doc/EliminationTree.ipynb | 2 +- gtsam/inference/doc/Factor.ipynb | 2 +- gtsam/inference/doc/FactorGraph.ipynb | 2 +- gtsam/inference/doc/ISAM.ipynb | 2 +- gtsam/inference/doc/JunctionTree.ipynb | 2 +- gtsam/inference/doc/Key.ipynb | 2 +- gtsam/inference/doc/LabeledSymbol.ipynb | 2 +- gtsam/inference/doc/Symbol.ipynb | 2 +- gtsam/inference/doc/VariableIndex.ipynb | 2 +- gtsam/nonlinear/doc/CustomFactor.ipynb | 14 +- 19 files changed, 670 insertions(+), 680 deletions(-) diff --git a/gtsam/geometry/doc/Pose2.ipynb b/gtsam/geometry/doc/Pose2.ipynb index 660c78ac6..8dc7ffccc 100644 --- a/gtsam/geometry/doc/Pose2.ipynb +++ b/gtsam/geometry/doc/Pose2.ipynb @@ -39,7 +39,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/geometry/doc/Pose3.ipynb b/gtsam/geometry/doc/Pose3.ipynb index 25a45fafb..2a925fb6d 100644 --- a/gtsam/geometry/doc/Pose3.ipynb +++ b/gtsam/geometry/doc/Pose3.ipynb @@ -30,7 +30,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/geometry/doc/Rot2.ipynb b/gtsam/geometry/doc/Rot2.ipynb index 610727090..f4eb642a2 100644 --- a/gtsam/geometry/doc/Rot2.ipynb +++ b/gtsam/geometry/doc/Rot2.ipynb @@ -1,45 +1,31 @@ { - "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" - } + }, + "source": [ + "# Rot2" + ] }, { "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" - } + }, + "source": [ + "A `gtsam.Rot2` represents rotation in 2D space. It models a 2D rotation in the Special Orthogonal Group $\\text{SO}(2)$." + ] }, { "cell_type": "markdown", - "source": [ - "\"Open" - ], "metadata": { "id": "1MUA6xip5fG4" - } + }, + "source": [ + "\"Open" + ] }, { "cell_type": "code", @@ -49,41 +35,70 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { "cell_type": "code", - "source": [ - "from gtsam import Rot2, Point2\n", - "import numpy as np" - ], + "execution_count": 6, "metadata": { "id": "-dp28DoR7WsD" }, - "execution_count": 6, - "outputs": [] + "outputs": [], + "source": [ + "from gtsam import Rot2, Point2\n", + "import numpy as np" + ] }, { "cell_type": "markdown", - "source": [ - "## Initialization and properties" - ], "metadata": { "id": "gZRXZTrJ7mqJ" - } + }, + "source": [ + "## Initialization and properties" + ] }, { "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" - } + }, + "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." + ] }, { "cell_type": "code", + "execution_count": 58, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "oIakIOAB9afi", + "outputId": "c2cb005d-056f-4a4f-b5ff-2226be1ba3e7" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "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" + ] + } + ], "source": [ "# The identity rotation has theta = 0.\n", "identity = Rot2()\n", @@ -118,39 +133,13 @@ "# 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", + "metadata": { + "id": "rHovUXbUys5r" + }, "source": [ "The following properties are available from the standard interface:\n", "- `theta()` (in radians)\n", @@ -161,25 +150,11 @@ "\\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" - ], + "execution_count": 18, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -187,11 +162,10 @@ "id": "P5OXTjFu2DeX", "outputId": "70848419-c055-44bc-de11-08e8f93fe3bf" }, - "execution_count": 18, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "theta: 2.35619\n", "\n", @@ -204,28 +178,59 @@ " [ 0.70710678 -0.70710678]]\n" ] } + ], + "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" ] }, { "cell_type": "markdown", - "source": [ - "## Basic operations" - ], "metadata": { "id": "PpqHUDnl5rTW" - } + }, + "source": [ + "## Basic operations" + ] }, { "cell_type": "markdown", - "source": [ - "For basic use, a `Rot2` can rotate and unrotate a point." - ], "metadata": { "id": "sa4qx58n5tG9" - } + }, + "source": [ + "For basic use, a `Rot2` can rotate and unrotate a point." + ] }, { "cell_type": "code", + "execution_count": 25, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "yaBKjGn05_-c", + "outputId": "fb89fe09-b2b8-496d-e835-f379d197a4eb" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Rotated: [-2.82842712e+00 2.22044605e-16]\n", + "Unrotated: [-2. 2.]\n", + "Unrotated again: [-2.22044605e-16 2.82842712e+00]\n" + ] + } + ], "source": [ "rot = Rot2.fromDegrees(45)\n", "p = Point2(-2, 2)\n", @@ -237,38 +242,37 @@ "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" - } + }, + "source": [ + "Also, the `equals()` function allows for comparison of two `Rot2` objects with a tolerance." + ] }, { "cell_type": "code", + "execution_count": 31, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "m74YbK5h6CPU", + "outputId": "1b16695e-cdfe-4348-875f-c41d8b4a3b26" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "True\n", + "False\n" + ] + } + ], "source": [ "eq_rads = Rot2(np.pi / 4)\n", "eq_degs = Rot2.fromDegrees(45)\n", @@ -277,48 +281,60 @@ "\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" - } + }, + "source": [ + "## Lie group $\\text{SO}(2)$" + ] }, { "cell_type": "markdown", + "metadata": { + "id": "76D2KkX241zX" + }, "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", + "execution_count": 57, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "QZ_NTeXK87Wq", + "outputId": "12af920d-8f86-473d-88ab-ee57d316cb72" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "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" + ] + } + ], "source": [ "a = Rot2(np.pi / 6)\n", "b = Rot2(np.pi / 3)\n", @@ -340,51 +356,44 @@ "# 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", + "metadata": { + "id": "YfiYuVpL-cgq" + }, "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", + "execution_count": 54, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "4JiDEGqG-van", + "outputId": "56047f8d-3349-4ee6-e73c-cd2fa1efb95d" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "theta: 1.5708\n", + "\n", + "theta: 3.14159\n", + "\n", + "[1.57079633]\n", + "[-0.78539816]\n", + "[-0.78539816]\n" + ] + } + ], "source": [ "r = Rot2(np.pi / 2)\n", "w = Rot2(np.pi / 4)\n", @@ -405,30 +414,21 @@ "# 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 + ], + "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/Rot3.ipynb b/gtsam/geometry/doc/Rot3.ipynb index 937679528..0eb21ddae 100644 --- a/gtsam/geometry/doc/Rot3.ipynb +++ b/gtsam/geometry/doc/Rot3.ipynb @@ -1,45 +1,31 @@ { - "nbformat": 4, - "nbformat_minor": 0, - "metadata": { - "colab": { - "provenance": [] - }, - "kernelspec": { - "name": "python3", - "display_name": "Python 3" - }, - "language_info": { - "name": "python" - } - }, "cells": [ { "cell_type": "markdown", - "source": [ - "# Rot3" - ], "metadata": { "id": "Wy0JIcGioHI9" - } + }, + "source": [ + "# Rot3" + ] }, { "cell_type": "markdown", - "source": [ - "A `gtsam.Rot3` represents an orientation or attitude in 3D space. It can be manipulated and presented as a rotation matrix $ R \\in \\mathbb{R}^{3 \\times 3} $, a unit quaternion, roll-pitch-yaw (Euler) angles $ (\\phi, \\theta, \\psi) $, or as an axis-angle representation $ (\\hat{\\omega}, \\theta) $ with $ \\hat{\\omega} \\in \\mathbb{R}^3 $ and $ \\theta \\in \\mathbb{R} $. It models a 3D orientation as both a manifold in $ \\mathcal{SO}(3) $ and as a Lie group in $ \\text{SO}(3) $. Internally, it is stored as a $ 3 \\times 3 $ rotation matrix but can be configured to use quaternions at build time for efficiency." - ], "metadata": { "id": "YqaxPKyloJG_" - } + }, + "source": [ + "A `gtsam.Rot3` represents an orientation or attitude in 3D space. It can be manipulated and presented as a rotation matrix $ R \\in \\mathbb{R}^{3 \\times 3} $, a unit quaternion, roll-pitch-yaw (Euler) angles $ (\\phi, \\theta, \\psi) $, or as an axis-angle representation $ (\\hat{\\omega}, \\theta) $ with $ \\hat{\\omega} \\in \\mathbb{R}^3 $ and $ \\theta \\in \\mathbb{R} $. It models a 3D orientation as both a manifold in $ \\mathcal{SO}(3) $ and as a Lie group in $ \\text{SO}(3) $. Internally, it is stored as a $ 3 \\times 3 $ rotation matrix but can be configured to use quaternions at build time for efficiency." + ] }, { "cell_type": "markdown", - "source": [ - "\"Open" - ], "metadata": { "id": "Hmwbhz75pcQT" - } + }, + "source": [ + "\"Open" + ] }, { "cell_type": "code", @@ -49,79 +35,54 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "_fWy46Mepoxh" + }, + "outputs": [], "source": [ "import gtsam\n", "from gtsam import Rot3, Point3, Quaternion\n", "import numpy as np" - ], - "metadata": { - "id": "_fWy46Mepoxh" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "## Initialization" - ], "metadata": { "id": "3DkkBKyAqGnY" - } + }, + "source": [ + "## Initialization" + ] }, { "cell_type": "markdown", - "source": [ - "A `Rot3` can be initialized in many different ways, which are detailed in this section. Note that printing a `Rot3` displays its 3x3 rotation matrix representation, which in general is a 3x3 matrix where the columns are unit vectors that define the orientation's coordinate frame." - ], "metadata": { "id": "RrJ5ZEdhqJPU" - } + }, + "source": [ + "A `Rot3` can be initialized in many different ways, which are detailed in this section. Note that printing a `Rot3` displays its 3x3 rotation matrix representation, which in general is a 3x3 matrix where the columns are unit vectors that define the orientation's coordinate frame." + ] }, { "cell_type": "markdown", + "metadata": { + "id": "AqEy5JLe5X1t" + }, "source": [ "### Constructor\n", "\n", "The `Rot3` constructor provides for initialization with no arguments, yielding the identity rotation (equivalent to $I_3$), initialization with a precalculated rotation matrix (either as a 3x3 `np.ndarray`, as three 3-vectors, or as 9 floats), and initialization with a quaternion's $w, x, y, z$." - ], - "metadata": { - "id": "AqEy5JLe5X1t" - } + ] }, { "cell_type": "code", - "source": [ - "# No-argument constructor\n", - "a = Rot3()\n", - "print(a)\n", - "\n", - "# Construct from a rotation matrix\n", - "theta = np.pi / 2\n", - "b = Rot3(np.array([ # Rotate around X axis by PI / 2\n", - " [1, 0, 0],\n", - " [0, np.cos(theta), -np.sin(theta)],\n", - " [0, np.sin(theta), np.cos(theta)]\n", - "]))\n", - "print(b)\n", - "\n", - "# Construct from three column vectors\n", - "c = Rot3([11, 21, 31], [12, 22, 32], [13, 23, 33])\n", - "print(c)\n", - "\n", - "# Construct from 9 floats\n", - "d = Rot3(1, 2, 3, 4, 5, 6, 7, 8, 9)\n", - "print(d)\n", - "\n", - "# Construct from quaternion values\n", - "e = Rot3(0, 0, 0, 1) # Rotate around Z axis by pi\n", - "print(e)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -129,11 +90,10 @@ "id": "mj8X-wIdq6GR", "outputId": "48a6921d-df39-4fd8-aaf8-76d4bcdb70b1" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -167,33 +127,57 @@ "\n" ] } + ], + "source": [ + "# No-argument constructor\n", + "a = Rot3()\n", + "print(a)\n", + "\n", + "# Construct from a rotation matrix\n", + "theta = np.pi / 2\n", + "b = Rot3(np.array([ # Rotate around X axis by PI / 2\n", + " [1, 0, 0],\n", + " [0, np.cos(theta), -np.sin(theta)],\n", + " [0, np.sin(theta), np.cos(theta)]\n", + "]))\n", + "print(b)\n", + "\n", + "# Construct from three column vectors\n", + "c = Rot3([11, 21, 31], [12, 22, 32], [13, 23, 33])\n", + "print(c)\n", + "\n", + "# Construct from 9 floats\n", + "d = Rot3(1, 2, 3, 4, 5, 6, 7, 8, 9)\n", + "print(d)\n", + "\n", + "# Construct from quaternion values\n", + "e = Rot3(0, 0, 0, 1) # Rotate around Z axis by pi\n", + "print(e)" ] }, { "cell_type": "markdown", + "metadata": { + "id": "EMaB3yVoJ_qv" + }, "source": [ "### Named constructors\n", "\n", "In addition to its constructors, `Rot3` has several named constructors, or factory functions, that allow instantiation from a wide variety of methods." - ], - "metadata": { - "id": "EMaB3yVoJ_qv" - } + ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.Identity()` returns the 3x3 rotation identity matrix." - ], "metadata": { "id": "3s9Ym_6BaE_r" - } + }, + "source": [ + "`Rot3.Identity()` returns the 3x3 rotation identity matrix." + ] }, { "cell_type": "code", - "source": [ - "print(Rot3.Identity())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -201,11 +185,10 @@ "id": "GcAB8GtVaLjK", "outputId": "b9e701cd-6a3f-4171-a518-158a2f7b60fd" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -215,19 +198,68 @@ "\n" ] } + ], + "source": [ + "print(Rot3.Identity())" ] }, { "cell_type": "markdown", - "source": [ - "`Rx`, `Ry`, `Rz`, and `RzRyRx` create rotations around these axes." - ], "metadata": { "id": "F2MXz29VXqLR" - } + }, + "source": [ + "`Rx`, `Ry`, `Rz`, and `RzRyRx` create rotations around these axes." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "w-qh5dX6VWAW", + "outputId": "8fe29ae6-eb47-4460-c27b-3acd8ee5e5bf" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 6.12323e-17, -1;\n", + "\t0, 1, 6.12323e-17\n", + "]\n", + "\n", + "R: [\n", + "\t0.707107, 0, 0.707107;\n", + "\t0, 1, 0;\n", + "\t-0.707107, 0, 0.707107\n", + "]\n", + "\n", + "R: [\n", + "\t0.866025, -0.5, 0;\n", + "\t0.5, 0.866025, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n", + "R: [\n", + "\t0.612372, 0.612372, 0.5;\n", + "\t0.353553, 0.353553, -0.866025;\n", + "\t-0.707107, 0.707107, 4.32978e-17\n", + "]\n", + "\n", + "R: [\n", + "\t0.612372, 0.612372, 0.5;\n", + "\t0.353553, 0.353553, -0.866025;\n", + "\t-0.707107, 0.707107, 4.32978e-17\n", + "]\n", + "\n" + ] + } + ], "source": [ "# Rotation around X axis\n", "x = Rot3.Rx(np.pi / 2)\n", @@ -250,24 +282,36 @@ "print(zyx)\n", "# Of course, zyx is the same as z * y * x, since we fed the same angles to each.\n", "print(z * y * x)" - ], + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "c-_H7XmUYAd_" + }, + "source": [ + "Similarly, `Yaw`, `Pitch`, `Roll`, and `Ypr` are available." + ] + }, + { + "cell_type": "code", + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" }, - "id": "w-qh5dX6VWAW", - "outputId": "8fe29ae6-eb47-4460-c27b-3acd8ee5e5bf" + "id": "bGEMGXkpYT9t", + "outputId": "31655b9f-045f-4b51-dff2-42de4382427f" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", - "\t1, 0, 0;\n", - "\t0, 6.12323e-17, -1;\n", - "\t0, 1, 6.12323e-17\n", + "\t0.866025, -0.5, 0;\n", + "\t0.5, 0.866025, 0;\n", + "\t0, 0, 1\n", "]\n", "\n", "R: [\n", @@ -277,15 +321,9 @@ "]\n", "\n", "R: [\n", - "\t0.866025, -0.5, 0;\n", - "\t0.5, 0.866025, 0;\n", - "\t0, 0, 1\n", - "]\n", - "\n", - "R: [\n", - "\t0.612372, 0.612372, 0.5;\n", - "\t0.353553, 0.353553, -0.866025;\n", - "\t-0.707107, 0.707107, 4.32978e-17\n", + "\t1, 0, 0;\n", + "\t0, 6.12323e-17, -1;\n", + "\t0, 1, 6.12323e-17\n", "]\n", "\n", "R: [\n", @@ -296,19 +334,7 @@ "\n" ] } - ] - }, - { - "cell_type": "markdown", - "source": [ - "Similarly, `Yaw`, `Pitch`, `Roll`, and `Ypr` are available." ], - "metadata": { - "id": "c-_H7XmUYAd_" - } - }, - { - "cell_type": "code", "source": [ "# Yaw around Z axis (positive yaw is to the right, as in aircraft heading)\n", "y = Rot3.Yaw(np.pi / 6)\n", @@ -327,65 +353,20 @@ "# Ypr is not overloaded to support an array.\n", "ypr = Rot3.Ypr(np.pi / 6, np.pi / 4, np.pi / 2)\n", "print(ypr)" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "bGEMGXkpYT9t", - "outputId": "31655b9f-045f-4b51-dff2-42de4382427f" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "R: [\n", - "\t0.866025, -0.5, 0;\n", - "\t0.5, 0.866025, 0;\n", - "\t0, 0, 1\n", - "]\n", - "\n", - "R: [\n", - "\t0.707107, 0, 0.707107;\n", - "\t0, 1, 0;\n", - "\t-0.707107, 0, 0.707107\n", - "]\n", - "\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 6.12323e-17, -1;\n", - "\t0, 1, 6.12323e-17\n", - "]\n", - "\n", - "R: [\n", - "\t0.612372, 0.612372, 0.5;\n", - "\t0.353553, 0.353553, -0.866025;\n", - "\t-0.707107, 0.707107, 4.32978e-17\n", - "]\n", - "\n" - ] - } ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.Quaternion` is identical to the four-argument `Rot3` constructor." - ], "metadata": { "id": "_ks-ohhZZ5Ap" - } + }, + "source": [ + "`Rot3.Quaternion` is identical to the four-argument `Rot3` constructor." + ] }, { "cell_type": "code", - "source": [ - "# Create from quaternion w, x, y, z\n", - "q = Rot3.Quaternion(0, 0, 0, 1)\n", - "print(q)\n", - "print(q.equals(Rot3(0, 0, 0, 1), 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -393,11 +374,10 @@ "id": "uO9hb2RBaG3g", "outputId": "5409ef2e-3651-439b-af9f-6ed7888aa9d5" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t-1, 0, 0;\n", @@ -408,23 +388,26 @@ "True\n" ] } + ], + "source": [ + "# Create from quaternion w, x, y, z\n", + "q = Rot3.Quaternion(0, 0, 0, 1)\n", + "print(q)\n", + "print(q.equals(Rot3(0, 0, 0, 1), 1e-8))" ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.AxisAngle` creates a `Rot3` from an axis and an angle around that axis." - ], "metadata": { "id": "jy7dn6_vabsK" - } + }, + "source": [ + "`Rot3.AxisAngle` creates a `Rot3` from an axis and an angle around that axis." + ] }, { "cell_type": "code", - "source": [ - "aa = Rot3.AxisAngle([0, 1, 0], np.pi / 2)\n", - "print(aa)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -432,11 +415,10 @@ "id": "M_OOSKgAaqhF", "outputId": "cbb875b7-9204-4a90-c225-dbea46718c6f" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t2.22045e-16, 0, 1;\n", @@ -446,25 +428,24 @@ "\n" ] } + ], + "source": [ + "aa = Rot3.AxisAngle([0, 1, 0], np.pi / 2)\n", + "print(aa)" ] }, { "cell_type": "markdown", - "source": [ - "`Rot3.Rodrigues` creates a `Rot3` from incremental roll, pitch, and yaw values. It is identical to the exponential map at identity." - ], "metadata": { "id": "ruyunRlUclFX" - } + }, + "source": [ + "`Rot3.Rodrigues` creates a `Rot3` from incremental roll, pitch, and yaw values. It is identical to the exponential map at identity." + ] }, { "cell_type": "code", - "source": [ - "rod = Rot3.Rodrigues(np.pi / 6, np.pi / 4, np.pi / 2)\n", - "# Rodrigues is overloaded to support an array.\n", - "# e.g. Rot3.Rodrigues([np.pi / 6, np.pi / 4, np.pi / 2])\n", - "print(rod)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -472,11 +453,10 @@ "id": "NUgeRQzIcqYp", "outputId": "8c189748-267b-4c25-e0cf-4eed8721bc4a" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t-0.156058, -0.673795, 0.72225;\n", @@ -486,31 +466,30 @@ "\n" ] } + ], + "source": [ + "rod = Rot3.Rodrigues(np.pi / 6, np.pi / 4, np.pi / 2)\n", + "# Rodrigues is overloaded to support an array.\n", + "# e.g. Rot3.Rodrigues([np.pi / 6, np.pi / 4, np.pi / 2])\n", + "print(rod)" ] }, { "cell_type": "markdown", + "metadata": { + "id": "INihgtF6fI22" + }, "source": [ "`Rot3.ClosestTo` finds the closest valid `Rot3` to the input matrix which minimizes the Frobenius norm. The Frobenius norm is a measure of matrix difference:\n", "\n", "$$\n", "||A - B||_F = \\sqrt{\\sum_{i,j} (A_{ij} - B_{ij})^2}\n", "$$" - ], - "metadata": { - "id": "INihgtF6fI22" - } + ] }, { "cell_type": "code", - "source": [ - "closest = Rot3.ClosestTo([\n", - " [1, 0, 0],\n", - " [0, 2, 0],\n", - " [0, 0, 3]\n", - "])\n", - "print(closest)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -518,11 +497,10 @@ "id": "EFMbwiTKfLfJ", "outputId": "72bf7784-6a8c-4052-c444-d85d6d9014e7" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -532,19 +510,30 @@ "\n" ] } + ], + "source": [ + "closest = Rot3.ClosestTo([\n", + " [1, 0, 0],\n", + " [0, 2, 0],\n", + " [0, 0, 3]\n", + "])\n", + "print(closest)" ] }, { "cell_type": "markdown", - "source": [ - "## Properties" - ], "metadata": { "id": "Sm3oUTObqxJl" - } + }, + "source": [ + "## Properties" + ] }, { "cell_type": "markdown", + "metadata": { + "id": "0Gax04WXqyeE" + }, "source": [ "The following properties are available from the standard interface:\n", "- `matrix()`: Returns the 3x3 rotation matrix.\n", @@ -559,31 +548,11 @@ "- `toQuaternion()`: Returns the quaternion representation. The quaternion's attributes can then be accessed either individually with `w()`, `x()`, `y()`, `z()` or together with `coeffs()`.\n", "\n", "Note that accessing `roll()`, `pitch()`, and `yaw()` separately is less efficient than calling `rpy()` or `ypr()`." - ], - "metadata": { - "id": "0Gax04WXqyeE" - } + ] }, { "cell_type": "code", - "source": [ - "props = Rot3.RzRyRx(0, np.pi / 6, np.pi / 2)\n", - "\n", - "print(\"Matrix:\\n\", props.matrix())\n", - "print(\"Transpose:\\n\", props.transpose())\n", - "print()\n", - "print(\"x, y, z:\", props.xyz())\n", - "print(\"y, p, r:\", props.ypr())\n", - "print(\"r, p, y:\", props.rpy())\n", - "print()\n", - "print(\"Roll: \", props.roll())\n", - "print(\"Pitch: \", props.pitch())\n", - "print(\"Yaw: \", props.yaw())\n", - "print()\n", - "print(\"Axis-angle:\\n\", props.axisAngle())\n", - "print()\n", - "print(\"Quaternion:\", props.toQuaternion().coeffs())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -591,11 +560,10 @@ "id": "zbNPBHiwDAE2", "outputId": "716f9db1-f7c7-4418-f7c7-5c6bae0b6a2c" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "Matrix:\n", " [[ 5.30287619e-17 -1.00000000e+00 3.06161700e-17]\n", @@ -623,28 +591,65 @@ "Quaternion: [-0.1830127 0.1830127 0.6830127 0.6830127]\n" ] } + ], + "source": [ + "props = Rot3.RzRyRx(0, np.pi / 6, np.pi / 2)\n", + "\n", + "print(\"Matrix:\\n\", props.matrix())\n", + "print(\"Transpose:\\n\", props.transpose())\n", + "print()\n", + "print(\"x, y, z:\", props.xyz())\n", + "print(\"y, p, r:\", props.ypr())\n", + "print(\"r, p, y:\", props.rpy())\n", + "print()\n", + "print(\"Roll: \", props.roll())\n", + "print(\"Pitch: \", props.pitch())\n", + "print(\"Yaw: \", props.yaw())\n", + "print()\n", + "print(\"Axis-angle:\\n\", props.axisAngle())\n", + "print()\n", + "print(\"Quaternion:\", props.toQuaternion().coeffs())" ] }, { "cell_type": "markdown", - "source": [ - "## Basic operations" - ], "metadata": { "id": "-XnTJ-psGeJf" - } + }, + "source": [ + "## Basic operations" + ] }, { "cell_type": "markdown", - "source": [ - "`Rot3` can rotate and unrotate a 3D point or vector. Rotation is calculated by the simple matrix product $Rx$, and unrotation by $R^{-1}x$." - ], "metadata": { "id": "gj3OlBlGGfjj" - } + }, + "source": [ + "`Rot3` can rotate and unrotate a 3D point or vector. Rotation is calculated by the simple matrix product $Rx$, and unrotation by $R^{-1}x$." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "rtbkHyp3GgWx", + "outputId": "43b1178c-39d3-4df2-face-9e8cef162cdd" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[1.2246468e-16 2.0000000e+00 0.0000000e+00]\n", + "[2. 0. 0.]\n", + "[ 1.2246468e-16 -2.0000000e+00 0.0000000e+00]\n" + ] + } + ], "source": [ "z90 = Rot3.Rz(np.pi / 2)\n", "point = [2, 0, 0]\n", @@ -656,38 +661,39 @@ "print(z90.unrotate(rotated))\n", "# Rotate backwards by 90 degrees around the Z axis\n", "print(z90.unrotate(point))" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "rtbkHyp3GgWx", - "outputId": "43b1178c-39d3-4df2-face-9e8cef162cdd" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "[1.2246468e-16 2.0000000e+00 0.0000000e+00]\n", - "[2. 0. 0.]\n", - "[ 1.2246468e-16 -2.0000000e+00 0.0000000e+00]\n" - ] - } ] }, { "cell_type": "markdown", - "source": [ - "Check whether two `Rot3` instances are equal within a certain tolerance using `equals()`. Be careful with the `==` operator; it does not compare rotational equivalence, it compares object reference. If you wish to use more fine-grained equality comparison, convert to `np.ndarray` with `matrix()`." - ], "metadata": { "id": "d0bQ-tmwHmZ5" - } + }, + "source": [ + "Check whether two `Rot3` instances are equal within a certain tolerance using `equals()`. Be careful with the `==` operator; it does not compare rotational equivalence, it compares object reference. If you wish to use more fine-grained equality comparison, convert to `np.ndarray` with `matrix()`." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "MYiKxq4vItz3", + "outputId": "e8f8fdd0-c539-476f-f233-2f78f98ca671" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "xyz.equals(ypr, 1e-8): True\n", + "xyz == ypr: False\n", + "xyz == xyz: True\n", + "xyz.matrix() == ypr.matrix(): True\n" + ] + } + ], "source": [ "xyz = Rot3.RzRyRx(np.pi / 2, np.pi / 4, np.pi / 6)\n", "ypr = Rot3.Ypr(np.pi / 6, np.pi / 4, np.pi / 2)\n", @@ -696,30 +702,13 @@ "print(\"xyz == ypr:\", xyz == ypr)\n", "print(\"xyz == xyz:\", xyz == xyz)\n", "print(\"xyz.matrix() == ypr.matrix():\", np.all(xyz.matrix() == ypr.matrix()))" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "MYiKxq4vItz3", - "outputId": "e8f8fdd0-c539-476f-f233-2f78f98ca671" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "xyz.equals(ypr, 1e-8): True\n", - "xyz == ypr: False\n", - "xyz == xyz: True\n", - "xyz.matrix() == ypr.matrix(): True\n" - ] - } ] }, { "cell_type": "markdown", + "metadata": { + "id": "bQzlRJ_2HWNz" + }, "source": [ "Use SLERP (spherical linear interpolation) to interpolate between two `Rot3` instances. In terms of the Lie algebra (see below), SLERP can be calculated by scaling the log mapped relative rotation by the interpolation term $t$, then converting back to $\\text{SO}(3)$ using the exponential map. The formula is thus:\n", "\n", @@ -728,19 +717,11 @@ "$$\n", "\n", "where $R_1$ and $R_2$ are the start `Rot3` and end `Rot3` of the interpolation and $t$ is the interpolation term, usually but not necessarily in the range $[0, 1]$." - ], - "metadata": { - "id": "bQzlRJ_2HWNz" - } + ] }, { "cell_type": "code", - "source": [ - "a = Rot3.RzRyRx(0, np.pi / 4, 0)\n", - "b = Rot3.RzRyRx(np.pi / 6, 0, 0)\n", - "\n", - "print(a.slerp(0.5, b))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -748,11 +729,10 @@ "id": "y45qZPivHkRR", "outputId": "e2320424-004f-47bf-e844-bf04df63a916" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.922613, 0.0523387, 0.382159;\n", @@ -762,36 +742,37 @@ "\n" ] } + ], + "source": [ + "a = Rot3.RzRyRx(0, np.pi / 4, 0)\n", + "b = Rot3.RzRyRx(np.pi / 6, 0, 0)\n", + "\n", + "print(a.slerp(0.5, b))" ] }, { "cell_type": "markdown", - "source": [ - "## Lie group $\\text{SO}(3)$" - ], "metadata": { "id": "XlmNuuxSGgoj" - } + }, + "source": [ + "## Lie group $\\text{SO}(3)$" + ] }, { "cell_type": "markdown", + "metadata": { + "id": "XtUmE-QSGsh9" + }, "source": [ "### Group operations\n", "\n", "`Rot3` 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": "XtUmE-QSGsh9" - } + ] }, { "cell_type": "code", - "source": [ - "a = Rot3.Rz(np.pi / 4)\n", - "b = Rot3.Roll(np.pi / 2)\n", - "\n", - "print(\"a:\\n\", a.matrix(), \"\\nb:\\n\", b.matrix())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -799,11 +780,10 @@ "id": "axvFPtxYdGru", "outputId": "977f9582-5b23-43c7-a6f5-a7bfce6d5cea" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "a:\n", " [[ 0.70710678 -0.70710678 0. ]\n", @@ -815,24 +795,26 @@ " [ 0.000000e+00 1.000000e+00 6.123234e-17]]\n" ] } + ], + "source": [ + "a = Rot3.Rz(np.pi / 4)\n", + "b = Rot3.Roll(np.pi / 2)\n", + "\n", + "print(\"a:\\n\", a.matrix(), \"\\nb:\\n\", b.matrix())" ] }, { "cell_type": "markdown", - "source": [ - "The inverse of an $\\text{SO}(3)$ rotation matrix is the same as its transpose." - ], "metadata": { "id": "3eH9K5VH9jTb" - } + }, + "source": [ + "The inverse of an $\\text{SO}(3)$ rotation matrix is the same as its transpose." + ] }, { "cell_type": "code", - "source": [ - "print(a.inverse())\n", - "# The inverse is the same as the transpose.\n", - "print(a.inverse().equals(Rot3(a.transpose()), 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -840,11 +822,10 @@ "id": "ffVBzuOhGugd", "outputId": "ff207bed-850c-422a-d9a6-a0b59e801989" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, 0.707107, 0;\n", @@ -855,28 +836,25 @@ "True\n" ] } + ], + "source": [ + "print(a.inverse())\n", + "# The inverse is the same as the transpose.\n", + "print(a.inverse().equals(Rot3(a.transpose()), 1e-8))" ] }, { "cell_type": "markdown", - "source": [ - "The product of the composition operation $A * B$ is the rotation matrix which applies the rotation of $A$ and then the rotation of $B$. The composition of two rotation matrices is just the product of the two matrices." - ], "metadata": { "id": "P1bYYGkGdjxm" - } + }, + "source": [ + "The product of the composition operation $A * B$ is the rotation matrix which applies the rotation of $A$ and then the rotation of $B$. The composition of two rotation matrices is just the product of the two matrices." + ] }, { "cell_type": "code", - "source": [ - "print(a.compose(b))\n", - "\n", - "# The * operator is syntactic sugar for the compose operation.\n", - "print(a.compose(b).equals(a * b, 1e-8))\n", - "\n", - "# The composition of two rotation matrices is just the product of the matrices.\n", - "print(np.all(a.compose(b).matrix() == a.matrix() @ b.matrix()))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -884,11 +862,10 @@ "id": "4zXJJ77FdLBB", "outputId": "c5875121-0d97-475c-8669-93b92ac37c1b" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, -4.32978e-17, 0.707107;\n", @@ -900,27 +877,33 @@ "True\n" ] } + ], + "source": [ + "print(a.compose(b))\n", + "\n", + "# The * operator is syntactic sugar for the compose operation.\n", + "print(a.compose(b).equals(a * b, 1e-8))\n", + "\n", + "# The composition of two rotation matrices is just the product of the matrices.\n", + "print(np.all(a.compose(b).matrix() == a.matrix() @ b.matrix()))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "TIuwUygjfECu" + }, "source": [ "The between operation calculates the rotation from one `Rot3` to another. It is defined as simply:\n", "\n", "$$\n", "R_{relative} = R_1^{-1}R_2\n", "$$" - ], - "metadata": { - "id": "TIuwUygjfECu" - } + ] }, { "cell_type": "code", - "source": [ - "print(a.between(b))\n", - "print(a.between(b).equals(a.inverse() * b, 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -928,11 +911,10 @@ "id": "_qGyDV15dgmU", "outputId": "be748925-3425-41c7-b06b-57ab87955699" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, 4.32978e-17, -0.707107;\n", @@ -943,22 +925,24 @@ "True\n" ] } + ], + "source": [ + "print(a.between(b))\n", + "print(a.between(b).equals(a.inverse() * b, 1e-8))" ] }, { "cell_type": "markdown", - "source": [ - "The identity is $I_3$, as described above." - ], "metadata": { "id": "6_jR6zhMfa1l" - } + }, + "source": [ + "The identity is $I_3$, as described above." + ] }, { "cell_type": "code", - "source": [ - "print(Rot3.Identity())" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -966,11 +950,10 @@ "id": "SchtjDIPfXtb", "outputId": "7d199a1e-b9a7-4775-81e7-9c1328012b89" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -980,26 +963,25 @@ "\n" ] } + ], + "source": [ + "print(Rot3.Identity())" ] }, { "cell_type": "markdown", + "metadata": { + "id": "fjfUIqKHflXY" + }, "source": [ "#### Group operation invariants\n", "\n", "See that the following group invariants hold:" - ], - "metadata": { - "id": "fjfUIqKHflXY" - } + ] }, { "cell_type": "code", - "source": [ - "print(\"Compose(a, Inverse(a)) == Identity: \", (a * a.inverse()).equals(Rot3.Identity(), 1e-8))\n", - "print(\"Compose(a, Between(a, b)) == b:\", (a * a.between(b)).equals(b, 1e-8))\n", - "print(\"Between(a, b) == Compose(Inverse(a), b):\", a.between(b).equals(a.inverse() * b, 1e-8))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1007,32 +989,39 @@ "id": "ysQSPxuwfnen", "outputId": "4a7d8404-fc2a-46ca-ba18-236fd417382b" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "Compose(a, Inverse(a)) == Identity: True\n", "Compose(a, Between(a, b)) == b: True\n", "Between(a, b) == Compose(Inverse(a), b): True\n" ] } + ], + "source": [ + "print(\"Compose(a, Inverse(a)) == Identity: \", (a * a.inverse()).equals(Rot3.Identity(), 1e-8))\n", + "print(\"Compose(a, Between(a, b)) == b:\", (a * a.between(b)).equals(b, 1e-8))\n", + "print(\"Between(a, b) == Compose(Inverse(a), b):\", a.between(b).equals(a.inverse() * b, 1e-8))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "nKxTJy8YGuxg" + }, "source": [ "### Lie group operations\n", "\n", "`Rot3` 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": "nKxTJy8YGuxg" - } + ] }, { "cell_type": "markdown", + "metadata": { + "id": "MmBfK0ad1KZ6" + }, "source": [ "The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). This is used to map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$.\n", "\n", @@ -1079,38 +1068,22 @@ "$$\n", "\n", "since $ \\sin\\theta \\approx \\theta$ and $ 1 - \\cos\\theta \\approx \\frac{\\theta^2}{2} $." - ], - "metadata": { - "id": "MmBfK0ad1KZ6" - } + ] }, { "cell_type": "code", - "source": [ - "r1 = Rot3.RzRyRx(np.pi / 6, np.pi / 2, 0)\n", - "r2 = Rot3.RzRyRx(0, 0, np.pi / 4)\n", - "p1 = [np.pi / 2, 0, 0]\n", - "\n", - "# The exponential map at identity creates a rotation using Rodrigues' formula.\n", - "print(Rot3.Expmap(p1))\n", - "# The retract function takes the exponential map of the supplied vector and\n", - "# composes it with the calling Rot3. In other words, it maps from the tangent\n", - "# space to the manifold.\n", - "print(r1)\n", - "print(r1.retract(p1))" - ], + "execution_count": null, "metadata": { - "id": "yA5wO-5jGw2u", "colab": { "base_uri": "https://localhost:8080/" }, + "id": "yA5wO-5jGw2u", "outputId": "e0c75e07-2b6d-4f84-a90d-f1cceb3ad9fa" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t1, 0, 0;\n", @@ -1132,10 +1105,26 @@ "\n" ] } + ], + "source": [ + "r1 = Rot3.RzRyRx(np.pi / 6, np.pi / 2, 0)\n", + "r2 = Rot3.RzRyRx(0, 0, np.pi / 4)\n", + "p1 = [np.pi / 2, 0, 0]\n", + "\n", + "# The exponential map at identity creates a rotation using Rodrigues' formula.\n", + "print(Rot3.Expmap(p1))\n", + "# The retract function takes the exponential map of the supplied vector and\n", + "# composes it with the calling Rot3. In other words, it maps from the tangent\n", + "# space to the manifold.\n", + "print(r1)\n", + "print(r1.retract(p1))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "Yk2nazsK6ixV" + }, "source": [ "The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (a Lie algebra element in $ \\mathfrak{so}(3) $).\n", "\n", @@ -1183,13 +1172,29 @@ "$$\n", "\n", "where $ R_{ij} $ are the elements of $ R $." - ], - "metadata": { - "id": "Yk2nazsK6ixV" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": { + "base_uri": "https://localhost:8080/" + }, + "id": "0V2oQQ0lxS2-", + "outputId": "62b40acb-799e-4a91-dacd-c9e0266665c3" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[ 0.41038024 1.53155991 -0.41038024]\n", + "[-1.01420581 -1.32173874 1.01420581]\n", + "[-1.01420581 -1.32173874 1.01420581]\n" + ] + } + ], "source": [ "# Calculate the log map of r at identity. Returns the coordinates of the rotation\n", "# in the tangent space.\n", @@ -1200,37 +1205,11 @@ "# logmap is the same as calculating the coordinate of the second Rot3 in the\n", "# local frame of the first, which localCoordinates (inherited from LieGroup) does.\n", "print(r1.localCoordinates(r2))" - ], - "metadata": { - "colab": { - "base_uri": "https://localhost:8080/" - }, - "id": "0V2oQQ0lxS2-", - "outputId": "62b40acb-799e-4a91-dacd-c9e0266665c3" - }, - "execution_count": null, - "outputs": [ - { - "output_type": "stream", - "name": "stdout", - "text": [ - "[ 0.41038024 1.53155991 -0.41038024]\n", - "[-1.01420581 -1.32173874 1.01420581]\n", - "[-1.01420581 -1.32173874 1.01420581]\n" - ] - } ] }, { "cell_type": "code", - "source": [ - "# Applying localCoordinates and then retract cancels out, returning r2 given any\n", - "# r1. This is because it transforms r2 from the manifold to the tangent space\n", - "# using the log map, then transforms that result back into the manifold using\n", - "# the exponential map.\n", - "print(r2)\n", - "print(r1.retract(r1.localCoordinates(r2)))" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1238,11 +1217,10 @@ "id": "-kTgSGJS06EC", "outputId": "97051c49-284e-4ee8-d806-53f0d842fc31" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "R: [\n", "\t0.707107, -0.707107, 0;\n", @@ -1258,34 +1236,30 @@ "\n" ] } + ], + "source": [ + "# Applying localCoordinates and then retract cancels out, returning r2 given any\n", + "# r1. This is because it transforms r2 from the manifold to the tangent space\n", + "# using the log map, then transforms that result back into the manifold using\n", + "# the exponential map.\n", + "print(r2)\n", + "print(r1.retract(r1.localCoordinates(r2)))" ] }, { "cell_type": "markdown", + "metadata": { + "id": "s_qu2bGt1-vr" + }, "source": [ "## Serialization\n", "\n", "A `Rot3` can be serialized to a string for saving, then later used by deserializing the string." - ], - "metadata": { - "id": "s_qu2bGt1-vr" - } + ] }, { "cell_type": "code", - "source": [ - "a = Rot3.Rx(np.pi / 2)\n", - "print(\"Before serialization:\", a)\n", - "\n", - "str_val = a.serialize()\n", - "print(str_val)\n", - "print(\"The serialized value is a string:\", type(str_val))\n", - "# Save to file, etc...\n", - "\n", - "b = Rot3()\n", - "b.deserialize(str_val)\n", - "print(\"After deserialization:\", b)" - ], + "execution_count": null, "metadata": { "colab": { "base_uri": "https://localhost:8080/" @@ -1293,11 +1267,10 @@ "id": "m6ku7L_768Ta", "outputId": "8cb6fe04-6759-4cd9-8145-42f4fc2a72dd" }, - "execution_count": null, "outputs": [ { - "output_type": "stream", "name": "stdout", + "output_type": "stream", "text": [ "Before serialization: R: [\n", "\t1, 0, 0;\n", @@ -1317,7 +1290,34 @@ "\n" ] } + ], + "source": [ + "a = Rot3.Rx(np.pi / 2)\n", + "print(\"Before serialization:\", a)\n", + "\n", + "str_val = a.serialize()\n", + "print(str_val)\n", + "print(\"The serialized value is a string:\", type(str_val))\n", + "# Save to file, etc...\n", + "\n", + "b = Rot3()\n", + "b.deserialize(str_val)\n", + "print(\"After deserialization:\", b)" ] } - ] -} \ No newline at end of file + ], + "metadata": { + "colab": { + "provenance": [] + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/inference/doc/BayesNet.ipynb b/gtsam/inference/doc/BayesNet.ipynb index c379b32ce..d6c75e9d6 100644 --- a/gtsam/inference/doc/BayesNet.ipynb +++ b/gtsam/inference/doc/BayesNet.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/BayesTree.ipynb b/gtsam/inference/doc/BayesTree.ipynb index a9452f4d4..324c8a56b 100644 --- a/gtsam/inference/doc/BayesTree.ipynb +++ b/gtsam/inference/doc/BayesTree.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Conditional.ipynb b/gtsam/inference/doc/Conditional.ipynb index 9f6a53665..c0fd371d3 100644 --- a/gtsam/inference/doc/Conditional.ipynb +++ b/gtsam/inference/doc/Conditional.ipynb @@ -63,7 +63,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/DotWriter.ipynb b/gtsam/inference/doc/DotWriter.ipynb index 6ad795add..97a73d476 100644 --- a/gtsam/inference/doc/DotWriter.ipynb +++ b/gtsam/inference/doc/DotWriter.ipynb @@ -40,7 +40,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/EdgeKey.ipynb b/gtsam/inference/doc/EdgeKey.ipynb index ca55d6619..df5d39684 100644 --- a/gtsam/inference/doc/EdgeKey.ipynb +++ b/gtsam/inference/doc/EdgeKey.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/EliminationTree.ipynb b/gtsam/inference/doc/EliminationTree.ipynb index 75378e7d7..5e42040a3 100644 --- a/gtsam/inference/doc/EliminationTree.ipynb +++ b/gtsam/inference/doc/EliminationTree.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Factor.ipynb b/gtsam/inference/doc/Factor.ipynb index 6e6ce1515..352bb95a5 100644 --- a/gtsam/inference/doc/Factor.ipynb +++ b/gtsam/inference/doc/Factor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/FactorGraph.ipynb b/gtsam/inference/doc/FactorGraph.ipynb index a48d56ef7..13acae2f5 100644 --- a/gtsam/inference/doc/FactorGraph.ipynb +++ b/gtsam/inference/doc/FactorGraph.ipynb @@ -53,7 +53,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/ISAM.ipynb b/gtsam/inference/doc/ISAM.ipynb index f4daa82a8..fcc5ba1f3 100644 --- a/gtsam/inference/doc/ISAM.ipynb +++ b/gtsam/inference/doc/ISAM.ipynb @@ -42,7 +42,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/JunctionTree.ipynb b/gtsam/inference/doc/JunctionTree.ipynb index b5835f9df..5aa0058a8 100644 --- a/gtsam/inference/doc/JunctionTree.ipynb +++ b/gtsam/inference/doc/JunctionTree.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Key.ipynb b/gtsam/inference/doc/Key.ipynb index c8ad5c8c3..cd8cc3382 100644 --- a/gtsam/inference/doc/Key.ipynb +++ b/gtsam/inference/doc/Key.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/LabeledSymbol.ipynb b/gtsam/inference/doc/LabeledSymbol.ipynb index c383cbbaa..a5051ea41 100644 --- a/gtsam/inference/doc/LabeledSymbol.ipynb +++ b/gtsam/inference/doc/LabeledSymbol.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/Symbol.ipynb b/gtsam/inference/doc/Symbol.ipynb index c9de75957..67c71972d 100644 --- a/gtsam/inference/doc/Symbol.ipynb +++ b/gtsam/inference/doc/Symbol.ipynb @@ -38,7 +38,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/inference/doc/VariableIndex.ipynb b/gtsam/inference/doc/VariableIndex.ipynb index 080e03fa1..ef1c9f41b 100644 --- a/gtsam/inference/doc/VariableIndex.ipynb +++ b/gtsam/inference/doc/VariableIndex.ipynb @@ -40,7 +40,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam-develop" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/nonlinear/doc/CustomFactor.ipynb b/gtsam/nonlinear/doc/CustomFactor.ipynb index beb2961bb..43d674919 100644 --- a/gtsam/nonlinear/doc/CustomFactor.ipynb +++ b/gtsam/nonlinear/doc/CustomFactor.ipynb @@ -18,7 +18,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "id": "5ccb48e4", "metadata": { "tags": [ @@ -28,17 +28,7 @@ "languageId": "markdown" } }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n", - "\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n", - "\u001b[0mNote: you may need to restart the kernel to use updated packages.\n" - ] - } - ], + "outputs": [], "source": [ "%pip install --quiet gtsam-develop" ]