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](\"https://colab.research.google.com/assets/colab-badge.svg\")
"
- ],
"metadata": {
"id": "1MUA6xip5fG4"
- }
+ },
+ "source": [
+ ""
+ ]
},
{
"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": [
- ""
- ],
"metadata": {
"id": "Hmwbhz75pcQT"
- }
+ },
+ "source": [
+ ""
+ ]
},
{
"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"
]