From 2f0bff14b78c245fa7eca3e070c3bd901a79f291 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 17 Apr 2025 10:28:17 -0400 Subject: [PATCH 1/2] Ordering minor fixes --- gtsam/inference/doc/Ordering.ipynb | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/doc/Ordering.ipynb b/gtsam/inference/doc/Ordering.ipynb index 9f4a595ed..e021677e8 100644 --- a/gtsam/inference/doc/Ordering.ipynb +++ b/gtsam/inference/doc/Ordering.ipynb @@ -17,7 +17,7 @@ "source": [ "An `Ordering` specifies the order in which variables are eliminated during inference (e.g., Gaussian elimination, multifrontal QR). The choice of ordering significantly impacts the computational cost and fill-in (sparsity) of the resulting Bayes net or Bayes tree.\n", "\n", - "GTSAM provides several algorithms to compute good orderings automatically, such as COLAMD and METIS (if available), or allows you to specify a custom ordering." + "GTSAM provides several algorithms to compute good orderings automatically, such as COLAMD and METIS, or allows you to specify a custom ordering." ] }, { @@ -40,7 +40,7 @@ }, "outputs": [], "source": [ - "%pip install gtsam" + "%pip install --quiet gtsam-develop" ] }, { @@ -139,7 +139,7 @@ "GTSAM provides algorithms to automatically compute an elimination ordering from a factor graph. Two common algorithms are:\n", "\n", "1. **COLAMD (Column Approximate Minimum Degree):** A greedy algorithm that aims to minimize *fill-in* at each elimination step. It typically produces orderings that are good for sparse direct methods executed sequentially.\n", - "2. **METIS:** A graph partitioning algorithm (requires METIS library to be installed when compiling GTSAM). It aims to find orderings that partition the graph well, often leading to more balanced elimination trees which can be beneficial for parallel computation and sometimes reduce overall fill-in compared to purely local greedy methods like COLAMD, especially on large, structured problems.\n", + "2. **METIS:** A graph partitioning algorithm. It aims to find orderings that partition the graph well, often leading to more balanced elimination trees which can be beneficial for parallel computation and sometimes reduce overall fill-in compared to purely local greedy methods like COLAMD, especially on large, structured problems.\n", "\n", "Let's illustrate the difference using a 2D grid factor graph." ] @@ -391,7 +391,7 @@ "source": [ "### COLAMD Ordering and Resulting Bayes Net\n", "\n", - "Now, we compute the COLAMD ordering and eliminate the variables according to this order. We then visualize the resulting Symbolic Bayes Net. The structure of the Bayes Net (specifically, the cliques formed by the conditional dependencies) reflects the structure of the elimination tree (or Bayes Tree)." + "Now, we compute the COLAMD ordering and eliminate the variables according to this order." ] }, { From 515127cbe90148e62b660f7e4ae7e679e4182272 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 17 Apr 2025 10:28:41 -0400 Subject: [PATCH 2/2] %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" ]