diff --git a/gtsam/geometry/doc/Rot2.ipynb b/gtsam/geometry/doc/Rot2.ipynb
index 704915b4c6..6107270905 100644
--- a/gtsam/geometry/doc/Rot2.ipynb
+++ b/gtsam/geometry/doc/Rot2.ipynb
@@ -67,7 +67,7 @@
     {
       "cell_type": "markdown",
       "source": [
-        "# Initialization and properties"
+        "## Initialization and properties"
       ],
       "metadata": {
         "id": "gZRXZTrJ7mqJ"
diff --git a/gtsam/geometry/doc/Rot3.ipynb b/gtsam/geometry/doc/Rot3.ipynb
new file mode 100644
index 0000000000..c0ba050673
--- /dev/null
+++ b/gtsam/geometry/doc/Rot3.ipynb
@@ -0,0 +1,1342 @@
+{
+  "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"
+      }
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "A `gtsam.Rot3` represents a rotation in 3D space. It can be manipulated and presented as a rotation matrix, a quaternion, roll-pitch-yaw angles (Euler angles), or as an axis-angle combination. It models a 3D rotation in the [Special Orthogonal Group $\\text{SO}(3)$](https://en.wikipedia.org/wiki/SO(3)), which is a [Lie group](https://en.wikipedia.org/wiki/Lie_group)."
+      ],
+      "metadata": {
+        "id": "YqaxPKyloJG_"
+      }
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Rot3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
+      ],
+      "metadata": {
+        "id": "Hmwbhz75pcQT"
+      }
+    },
+    {
+      "cell_type": "code",
+      "execution_count": 1,
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "HyXPOMakoDkY",
+        "outputId": "665a19a8-4265-4d54-bc43-b5f4df9b62c8"
+      },
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "Collecting gtsam\n",
+            "  Downloading gtsam-4.2-cp311-cp311-manylinux2014_x86_64.whl.metadata (7.6 kB)\n",
+            "Requirement already satisfied: numpy>=1.11.0 in /usr/local/lib/python3.11/dist-packages (from gtsam) (1.26.4)\n",
+            "Requirement already satisfied: pyparsing>=2.4.2 in /usr/local/lib/python3.11/dist-packages (from gtsam) (3.2.1)\n",
+            "Downloading gtsam-4.2-cp311-cp311-manylinux2014_x86_64.whl (22.4 MB)\n",
+            "\u001b[2K   \u001b[90m━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━\u001b[0m \u001b[32m22.4/22.4 MB\u001b[0m \u001b[31m10.7 MB/s\u001b[0m eta \u001b[36m0:00:00\u001b[0m\n",
+            "\u001b[?25hInstalling collected packages: gtsam\n",
+            "Successfully installed gtsam-4.2\n"
+          ]
+        }
+      ],
+      "source": [
+        "%pip install gtsam"
+      ]
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "import gtsam\n",
+        "from gtsam import Rot3, Point3, Quaternion\n",
+        "import numpy as np"
+      ],
+      "metadata": {
+        "id": "_fWy46Mepoxh"
+      },
+      "execution_count": 2,
+      "outputs": []
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "## Initialization"
+      ],
+      "metadata": {
+        "id": "3DkkBKyAqGnY"
+      }
+    },
+    {
+      "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 a coordinate frame."
+      ],
+      "metadata": {
+        "id": "RrJ5ZEdhqJPU"
+      }
+    },
+    {
+      "cell_type": "markdown",
+      "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)"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "mj8X-wIdq6GR",
+        "outputId": "48a6921d-df39-4fd8-aaf8-76d4bcdb70b1"
+      },
+      "execution_count": 3,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 1, 0;\n",
+            "\t0, 0, 1\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",
+            "\t11, 12, 13;\n",
+            "\t21, 22, 23;\n",
+            "\t31, 32, 33\n",
+            "]\n",
+            "\n",
+            "R: [\n",
+            "\t1, 2, 3;\n",
+            "\t4, 5, 6;\n",
+            "\t7, 8, 9\n",
+            "]\n",
+            "\n",
+            "R: [\n",
+            "\t-1, 0, 0;\n",
+            "\t0, -1, 0;\n",
+            "\t0, 0, 1\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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"
+      }
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "print(Rot3.Identity())"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "GcAB8GtVaLjK",
+        "outputId": "b9e701cd-6a3f-4171-a518-158a2f7b60fd"
+      },
+      "execution_count": 4,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 1, 0;\n",
+            "\t0, 0, 1\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "`Rx`, `Ry`, `Rz`, and `RzRyRx` create rotations around these axes."
+      ],
+      "metadata": {
+        "id": "F2MXz29VXqLR"
+      }
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "# Rotation around X axis\n",
+        "x = Rot3.Rx(np.pi / 2)\n",
+        "print(x)\n",
+        "\n",
+        "# Rotation around Y axis\n",
+        "y = Rot3.Ry(np.pi / 4)\n",
+        "print(y)\n",
+        "\n",
+        "# Rotation around Z axis\n",
+        "z = Rot3.Rz(np.pi / 6)\n",
+        "print(z)\n",
+        "\n",
+        "# Rotate around Z, then Y, then X axes\n",
+        "# Note that the order of angles in the function signature (x, y, z) is the\n",
+        "# reverse of the order in which the rotations are applied (z, y, x).\n",
+        "zyx = Rot3.RzRyRx(np.pi / 2, np.pi / 4, np.pi / 6)\n",
+        "# Rot3.RzRyRx is overloaded: it also accepts an array of [x, y, z].\n",
+        "# e.g. the above is identical to zyx = Rot3.RzRyRx([np.pi / 2, np.pi / 4, np.pi / 6]).\n",
+        "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)"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "w-qh5dX6VWAW",
+        "outputId": "8fe29ae6-eb47-4460-c27b-3acd8ee5e5bf"
+      },
+      "execution_count": 5,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "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"
+          ]
+        }
+      ]
+    },
+    {
+      "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",
+        "print(y)\n",
+        "\n",
+        "# Pitch around Y axis (positive pitch is up, as in increasing aircraft altitude)\n",
+        "p = Rot3.Pitch(np.pi / 4)\n",
+        "print(p)\n",
+        "\n",
+        "# Roll around X axis\n",
+        "r = Rot3.Roll(np.pi / 2)\n",
+        "print(r)\n",
+        "\n",
+        "# Yaw, pitch, then roll\n",
+        "# Unlike RzRyRx, rotations are applied in the same order as supplied.\n",
+        "# 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": 6,
+      "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"
+      }
+    },
+    {
+      "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))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "uO9hb2RBaG3g",
+        "outputId": "5409ef2e-3651-439b-af9f-6ed7888aa9d5"
+      },
+      "execution_count": 7,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t-1, 0, 0;\n",
+            "\t0, -1, 0;\n",
+            "\t0, 0, 1\n",
+            "]\n",
+            "\n",
+            "True\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "`Rot3.AxisAngle` creates a `Rot3` from an axis and an angle around that axis."
+      ],
+      "metadata": {
+        "id": "jy7dn6_vabsK"
+      }
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "aa = Rot3.AxisAngle([0, 1, 0], np.pi / 2)\n",
+        "print(aa)"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "M_OOSKgAaqhF",
+        "outputId": "cbb875b7-9204-4a90-c225-dbea46718c6f"
+      },
+      "execution_count": 8,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t2.22045e-16, 0, 1;\n",
+            "\t0, 1, 0;\n",
+            "\t-1, 0, 2.22045e-16\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "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"
+      }
+    },
+    {
+      "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)"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "NUgeRQzIcqYp",
+        "outputId": "8c189748-267b-4c25-e0cf-4eed8721bc4a"
+      },
+      "execution_count": 9,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t-0.156058, -0.673795, 0.72225;\n",
+            "\t0.982078, -0.0276074, 0.186445;\n",
+            "\t-0.105686, 0.738402, 0.666028\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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)"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "EFMbwiTKfLfJ",
+        "outputId": "72bf7784-6a8c-4052-c444-d85d6d9014e7"
+      },
+      "execution_count": 10,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 1, 0;\n",
+            "\t0, 0, 1\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "## Properties"
+      ],
+      "metadata": {
+        "id": "Sm3oUTObqxJl"
+      }
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "The following properties are available from the standard interface:\n",
+        "- `matrix()`: Returns the 3x3 rotation matrix.\n",
+        "- `transpose()`: Returns the transpose of the 3x3 rotation matrix.\n",
+        "- `xyz()`: Returns the 3 Euler angles as $x, y, z$.\n",
+        "- `ypr()`: Returns the 3 Euler angles as yaw, pitch, and roll.\n",
+        "- `rpy()`: Returns the 3 Euler angles as roll, pitch, and yaw.\n",
+        "- `roll()`: Returns the roll angle.\n",
+        "- `pitch()`: Returns the pitch angle.\n",
+        "- `yaw()`: Returns the yaw angle.\n",
+        "- `axisAngle()`: Returns the axis-angle representation.\n",
+        "- `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())"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "zbNPBHiwDAE2",
+        "outputId": "716f9db1-f7c7-4418-f7c7-5c6bae0b6a2c"
+      },
+      "execution_count": 11,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "Matrix:\n",
+            " [[ 5.30287619e-17 -1.00000000e+00  3.06161700e-17]\n",
+            " [ 8.66025404e-01  6.12323400e-17  5.00000000e-01]\n",
+            " [-5.00000000e-01  0.00000000e+00  8.66025404e-01]]\n",
+            "Transpose:\n",
+            " [[ 5.30287619e-17  8.66025404e-01 -5.00000000e-01]\n",
+            " [-1.00000000e+00  6.12323400e-17  0.00000000e+00]\n",
+            " [ 3.06161700e-17  5.00000000e-01  8.66025404e-01]]\n",
+            "\n",
+            "x, y, z: [0.         0.52359878 1.57079633]\n",
+            "y, p, r: [1.57079633 0.52359878 0.        ]\n",
+            "r, p, y: [0.         0.52359878 1.57079633]\n",
+            "\n",
+            "Roll:    0.0\n",
+            "Pitch:   0.5235987755982988\n",
+            "Yaw:     1.5707963267948966\n",
+            "\n",
+            "Axis-angle:\n",
+            " (:-0.250563\n",
+            " 0.250563\n",
+            " 0.935113\n",
+            ", 1.6378338249998232)\n",
+            "\n",
+            "Quaternion: [-0.1830127  0.1830127  0.6830127  0.6830127]\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "## Basic operations"
+      ],
+      "metadata": {
+        "id": "-XnTJ-psGeJf"
+      }
+    },
+    {
+      "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"
+      }
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "z90 = Rot3.Rz(np.pi / 2)\n",
+        "point = [2, 0, 0]\n",
+        "\n",
+        "# Rotate by 90 degrees around the Z axis\n",
+        "rotated = z90.rotate(point)\n",
+        "print(rotated)\n",
+        "# Undo the rotation\n",
+        "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": 12,
+      "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"
+      }
+    },
+    {
+      "cell_type": "code",
+      "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",
+        "\n",
+        "print(\"xyz.equals(ypr, 1e-8):\", xyz.equals(ypr, 1e-8))\n",
+        "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": 13,
+      "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",
+      "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",
+        "$$\n",
+        "R(t) = R_1 \\exp(t \\cdot \\log(R_1^{-1}R_2))\n",
+        "$$\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))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "y45qZPivHkRR",
+        "outputId": "e2320424-004f-47bf-e844-bf04df63a916"
+      },
+      "execution_count": 14,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t0.922613, 0.0523387, 0.382159;\n",
+            "\t0.0523387, 0.964602, -0.258464;\n",
+            "\t-0.382159, 0.258464, 0.887215\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "## Lie group $\\text{SO}(3)$"
+      ],
+      "metadata": {
+        "id": "XlmNuuxSGgoj"
+      }
+    },
+    {
+      "cell_type": "markdown",
+      "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())"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "axvFPtxYdGru",
+        "outputId": "977f9582-5b23-43c7-a6f5-a7bfce6d5cea"
+      },
+      "execution_count": 15,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "a:\n",
+            " [[ 0.70710678 -0.70710678  0.        ]\n",
+            " [ 0.70710678  0.70710678  0.        ]\n",
+            " [ 0.          0.          1.        ]] \n",
+            "b:\n",
+            " [[ 1.000000e+00  0.000000e+00  0.000000e+00]\n",
+            " [ 0.000000e+00  6.123234e-17 -1.000000e+00]\n",
+            " [ 0.000000e+00  1.000000e+00  6.123234e-17]]\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "The inverse of an $\\text{SO}(3)$ rotation matrix is the same as its transpose."
+      ],
+      "metadata": {
+        "id": "3eH9K5VH9jTb"
+      }
+    },
+    {
+      "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))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "ffVBzuOhGugd",
+        "outputId": "ff207bed-850c-422a-d9a6-a0b59e801989"
+      },
+      "execution_count": 16,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t0.707107, 0.707107, 0;\n",
+            "\t-0.707107, 0.707107, 0;\n",
+            "\t0, 0, 1\n",
+            "]\n",
+            "\n",
+            "True\n"
+          ]
+        }
+      ]
+    },
+    {
+      "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"
+      }
+    },
+    {
+      "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()))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "4zXJJ77FdLBB",
+        "outputId": "c5875121-0d97-475c-8669-93b92ac37c1b"
+      },
+      "execution_count": 17,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t0.707107, -4.32978e-17, 0.707107;\n",
+            "\t0.707107, 4.32978e-17, -0.707107;\n",
+            "\t0, 1, 6.12323e-17\n",
+            "]\n",
+            "\n",
+            "True\n",
+            "True\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "_qGyDV15dgmU",
+        "outputId": "be748925-3425-41c7-b06b-57ab87955699"
+      },
+      "execution_count": 18,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t0.707107, 4.32978e-17, -0.707107;\n",
+            "\t-0.707107, 4.32978e-17, -0.707107;\n",
+            "\t0, 1, 6.12323e-17\n",
+            "]\n",
+            "\n",
+            "True\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "source": [
+        "The identity is $I_3$, as described above."
+      ],
+      "metadata": {
+        "id": "6_jR6zhMfa1l"
+      }
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "print(Rot3.Identity())"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "SchtjDIPfXtb",
+        "outputId": "7d199a1e-b9a7-4775-81e7-9c1328012b89"
+      },
+      "execution_count": 19,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 1, 0;\n",
+            "\t0, 0, 1\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "ysQSPxuwfnen",
+        "outputId": "4a7d8404-fc2a-46ca-ba18-236fd417382b"
+      },
+      "execution_count": 20,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "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"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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",
+      "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",
+        "Given a rotation vector $\\boldsymbol{\\omega} = (\\omega_x, \\omega_y, \\omega_z)$, define:\n",
+        "\n",
+        "- The rotation axis as the unit vector:\n",
+        "  \n",
+        "  $$\n",
+        "  \\hat{\\omega} = \\frac{\\boldsymbol{\\omega}}{\\theta}, \\quad \\text{where } \\theta = \\|\\boldsymbol{\\omega}\\|\n",
+        "  $$\n",
+        "\n",
+        "- The skew-symmetric matrix $[\\boldsymbol{\\omega}]_\\times$:\n",
+        "\n",
+        "  $$\n",
+        "  [\\boldsymbol{\\omega}]_\\times =\n",
+        "  \\begin{bmatrix}\n",
+        "  0 & -\\omega_z & \\omega_y \\\\\n",
+        "  \\omega_z & 0 & -\\omega_x \\\\\n",
+        "  -\\omega_y & \\omega_x & 0\n",
+        "  \\end{bmatrix}\n",
+        "  $$\n",
+        "\n",
+        "Using Rodrigues' formula, the exponential map is:\n",
+        "\n",
+        "$$\n",
+        "\\exp([\\boldsymbol{\\omega}]_\\times) = I + \\frac{\\sin\\theta}{\\theta} [\\boldsymbol{\\omega}]_\\times + \\frac{1 - \\cos\\theta}{\\theta^2} [\\boldsymbol{\\omega}]_\\times^2\n",
+        "$$\n",
+        "\n",
+        "which results in the rotation matrix:\n",
+        "\n",
+        "$$\n",
+        "R = I + \\frac{\\sin\\theta}{\\theta} [\\boldsymbol{\\omega}]_\\times + \\frac{1 - \\cos\\theta}{\\theta^2} [\\boldsymbol{\\omega}]_\\times^2\n",
+        "$$\n",
+        "\n",
+        "where:\n",
+        "- $ I $ is the $ 3 \\times 3 $ identity matrix.\n",
+        "- $ \\theta $ is the magnitude of the rotation vector (rotation angle).\n",
+        "- $ [\\boldsymbol{\\omega}]_\\times $ represents the skew-symmetric matrix, the infinitesimal rotation generator.\n",
+        "\n",
+        "For very small $ \\theta $, we use the Taylor series approximation:\n",
+        "\n",
+        "$$\n",
+        "R \\approx I + [\\boldsymbol{\\omega}]_\\times\n",
+        "$$\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))"
+      ],
+      "metadata": {
+        "id": "yA5wO-5jGw2u",
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "outputId": "e0c75e07-2b6d-4f84-a90d-f1cceb3ad9fa"
+      },
+      "execution_count": 25,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 2.22045e-16, -1;\n",
+            "\t0, 1, 2.22045e-16\n",
+            "]\n",
+            "\n",
+            "R: [\n",
+            "\t6.12323e-17, 0.5, 0.866025;\n",
+            "\t0, 0.866025, -0.5;\n",
+            "\t-1, 3.06162e-17, 5.30288e-17\n",
+            "]\n",
+            "\n",
+            "R: [\n",
+            "\t6.12323e-17, 0.866025, -0.5;\n",
+            "\t0, -0.5, -0.866025;\n",
+            "\t-1, 5.30288e-17, -3.06162e-17\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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",
+        "Given a rotation matrix $ R $, the corresponding rotation vector $ \\boldsymbol{\\omega} \\in \\mathbb{R}^3 $ is computed as:\n",
+        "\n",
+        "$$\n",
+        "\\boldsymbol{\\omega} = \\theta \\hat{\\omega}\n",
+        "$$\n",
+        "\n",
+        "where:\n",
+        "- $ \\theta $ is the rotation angle:\n",
+        "  \n",
+        "  $$\n",
+        "  \\theta = \\cos^{-1} \\left( \\frac{\\text{Tr}(R) - 1}{2} \\right)\n",
+        "  $$\n",
+        "\n",
+        "- $ \\hat{\\omega} $ is the rotation axis, obtained from the skew-symmetric matrix $ [\\boldsymbol{\\omega}]_\\times $:\n",
+        "\n",
+        "  $$\n",
+        "  [\\boldsymbol{\\omega}]_\\times = \\frac{\\theta}{2 \\sin\\theta} (R - R^T)\n",
+        "  $$\n",
+        "\n",
+        "- Extracting the components:\n",
+        "\n",
+        "  $$\n",
+        "  \\boldsymbol{\\omega} =\n",
+        "  \\frac{\\theta}{2 \\sin\\theta} (R - R^T)^\\vee\n",
+        "  $$\n",
+        "\n",
+        "For small $ \\theta $, we use the Taylor series approximation:\n",
+        "\n",
+        "$$\n",
+        "\\boldsymbol{\\omega} \\approx \\frac{1}{2} (R - R^T)^\\vee\n",
+        "$$\n",
+        "\n",
+        "In both cases, $ (R - R^T)^\\vee $ extracts the unique 3D vector from a skew-symmetric matrix:\n",
+        "\n",
+        "$$\n",
+        "(R - R^T)^\\vee=\n",
+        "\\begin{bmatrix}\n",
+        "  R_{32} - R_{23} \\\\\n",
+        "  R_{13} - R_{31} \\\\\n",
+        "  R_{21} - R_{12}\n",
+        "  \\end{bmatrix}\n",
+        "$$\n",
+        "\n",
+        "where $ R_{ij} $ are the elements of $ R $."
+      ],
+      "metadata": {
+        "id": "Yk2nazsK6ixV"
+      }
+    },
+    {
+      "cell_type": "code",
+      "source": [
+        "# Calculate the log map of r at identity. Returns the coordinates of the rotation\n",
+        "# in the tangent space.\n",
+        "print(Rot3.Logmap(r1))\n",
+        "\n",
+        "# Transform r2 into its vector representation relative to r1.\n",
+        "print(r1.logmap(r2))\n",
+        "# 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": 26,
+      "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)))"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "-kTgSGJS06EC",
+        "outputId": "97051c49-284e-4ee8-d806-53f0d842fc31"
+      },
+      "execution_count": 27,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "R: [\n",
+            "\t0.707107, -0.707107, 0;\n",
+            "\t0.707107, 0.707107, 0;\n",
+            "\t-0, 0, 1\n",
+            "]\n",
+            "\n",
+            "R: [\n",
+            "\t0.707107, -0.707107, 9.04269e-17;\n",
+            "\t0.707107, 0.707107, 4.0637e-17;\n",
+            "\t-6.77245e-17, 6.77245e-17, 1\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    },
+    {
+      "cell_type": "markdown",
+      "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)"
+      ],
+      "metadata": {
+        "colab": {
+          "base_uri": "https://localhost:8080/"
+        },
+        "id": "m6ku7L_768Ta",
+        "outputId": "8cb6fe04-6759-4cd9-8145-42f4fc2a72dd"
+      },
+      "execution_count": 28,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "name": "stdout",
+          "text": [
+            "Before serialization: R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 6.12323e-17, -1;\n",
+            "\t0, 1, 6.12323e-17\n",
+            "]\n",
+            "\n",
+            "22 serialization::archive 15 1 0\n",
+            "0 1.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 6.12323399573676604e-17 -1.00000000000000000e+00 0.00000000000000000e+00 1.00000000000000000e+00 6.12323399573676604e-17\n",
+            "\n",
+            "The serialized value is a string: <class 'str'>\n",
+            "After deserialization: R: [\n",
+            "\t1, 0, 0;\n",
+            "\t0, 6.12323e-17, -1;\n",
+            "\t0, 1, 6.12323e-17\n",
+            "]\n",
+            "\n"
+          ]
+        }
+      ]
+    }
+  ]
+}
\ No newline at end of file