S61_driving_state.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "view-in-github",
    "tags": [
     "no-tex"
    ]
   },
   "source": [
    "<a href=\"https://colab.research.google.com/github/gtbook/robotics/blob/main/S61_driving_state.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "JoW4C_OkOMhe",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install -q -U gtbook\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "id": "10-snNDwOSuC",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "import math\n",
    "import gtsam\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "```{index} state; 2D pose space\n",
    "```\n",
    "\n",
    "# Planar Geometry\n",
    "\n",
    "> Linear algebra can be used to deal efficiently with rotation and translations in the plane.\n",
    "\n",
    "<img src=\"Figures6/S61-Autonomous_Vehicle_with_LIDAR_and_cameras-07.jpg\" alt=\"Splash image with steampunk autonomous car\" width=\"40%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous chapter, we saw that the configuration space for a differential drive robot\n",
    "can be represented as  ${\\cal Q} = \\mathbb{R}^2 \\times [0, 2\\pi),$\n",
    "and we used $q = (x,y,\\theta)$ to parameterize this configuration space.\n",
    "This choice of parameterization has the nice property of being minimal: \n",
    "${\\cal Q}$ is a three-dimensional object, and therefore at least three\n",
    "parameters are required to represent arbitrary configurations.\n",
    "Unfortunately, this choice of parameterization doesn't help much when reasoning\n",
    "about the geometry of the robot and its motion.\n",
    "Recall, for example, the ad hoc planar geometry we used to derive coordinates of points\n",
    "on the DDR as a function of $x,y,\\theta$.\n",
    "In this section, we use tools from linear algebra to represent and manipulate configurations.\n",
    "We begin by focusing our attention on representing orientation.\n",
    "We then extend our methods to include translation, and develop methods\n",
    "to describe relative configurations of a robot, which could be used to describe\n",
    "sample points along a continuous trajectory in $\\cal Q$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Planar Rotations\n",
    "\n",
    "> Rotation in the plane can be represented using $2\\times 2$ rotation matrices.\n",
    "\n",
    "In the previous chapter, we represented the orientation of the robot simply by $\\theta \\in [0,2\\pi)$.\n",
    "There are two main drawbacks to this choice.\n",
    "First, the mapping from the robot's orientation to the parameter $\\theta$ is not continuous.\n",
    "If the robot's orientation is $\\theta = 2\\pi - \\epsilon$, for example, and we rotate the robot counterclockwise\n",
    "(decreasing $\\epsilon$ to zero), we will encounter a discontinuity when $\\theta$ goes from very nearly $2\\pi$ to zero.  This will occur regardless of the initial\n",
    "value we choose for $\\epsilon$: when $\\epsilon$ reaches zero, there will be a jump in the value of $\\theta$.\n",
    "We call this jump a $singularity$.\n",
    "The second drawback is that parameterizing orientation by a single angle $\\theta$ does not generalize in a straightforward way to representing orientation in 3D.\n",
    "Further, even if we go to the effort to make this generalization work, the problems with singularities\n",
    "are complex for 3-dimensional rotations, and can cause serious problems if not handled properly."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<figure id=\"fig:2drotation\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/2D-rotation.PNG?raw=1\" style=\"width:10cm\" alt=\"\">\n",
    "<figcaption>Two coordinate frames that share a common origin.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Both difficulties above stem from the use of a minimal representation of orientation.\n",
    "To solve these problems, we'll introduce a somewhat redundant representation that explicity\n",
    "encodes the orientation of the axes of a target coordinate frame relative to a reference coordinate frame.\n",
    "Consider the two coordinate frames shown in the figure above.\n",
    "These two frames share a common origin, and Frame 1 is obtained by rotating by an angle $\\theta$ w.r.t.\n",
    "frame 0.\n",
    "We can express the $x$-axis of Frame 1 in coordinates relative to Frame 0\n",
    "by projecting $x_1$ onto $x_0$ and $y_0$.\n",
    "Likewise,\n",
    "we can express the $y$-axis of Frame 1 w.r.t. Frame 0\n",
    "by projecting $y_1$ onto $x_0$ and $y_0$.\n",
    "Because all of these vectors are unit vectors,\n",
    "the projections can be achieved using the dot product:\n",
    "\n",
    "$$\n",
    "x_{1}^{0}=\n",
    "\\begin{bmatrix}\n",
    "x_0 \\cdot x_1 \\\\\n",
    "y_0 \\cdot x_1\n",
    "\\end{bmatrix}=\n",
    "\\begin{bmatrix}\n",
    "\\cos \\theta \\\\\n",
    "\\sin \\theta\n",
    "\\end{bmatrix}\n",
    "~~~~~~~~~~~\n",
    "y_{1}^{0}=\n",
    "\\begin{bmatrix}\n",
    "x_0 \\cdot y_1 \\\\\n",
    "y_0 \\cdot y_1\n",
    "\\end{bmatrix}=\n",
    "\\begin{bmatrix}\n",
    "-\\sin \\theta \\\\\n",
    "\\cos \\theta\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Above, we adopt the notational convention that a subscript indicates the target\n",
    "frame, and the superscript indicates the reference frame.\n",
    "Thus, $x_1^0$ denotes the $x$-axis of Frame 1, expressed in coordinates relative to Frame 0."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} rotation matrix\n",
    "```\n",
    "We can group these two vectors into a **rotation matrix**\n",
    "\n",
    "$$\n",
    "R_{1}^{0}\n",
    "=\\begin{bmatrix}\n",
    "x_0 \\cdot x_1 &  x_0 \\cdot y_1\\\\\n",
    "y_0 \\cdot x_1 & y_0 \\cdot y_1\n",
    "\\end{bmatrix}\n",
    "=\\begin{bmatrix}\n",
    "\\cos \\theta &  -\\sin \\theta\\\\\n",
    "\\sin \\theta & \\cos \\theta\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "that describes the orientation of Frame 1 w.r.t. Frame 0.\n",
    "Using the notational convention defined above, $R_{1}^{0}$ indicates the reference frame\n",
    "as the superscript (Frame 0) and the target frame as the subscript (Frame 1)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} special orthogonal group of order 2\n",
    "```\n",
    "## The Special Orthogonal Group of Order 2, aka SO(2)\n",
    "\n",
    "> Rotation matrices form a group.\n",
    "\n",
    "The set of $2\\times2$ rotation matrices, together with matrix multiplication as the\n",
    "composition operator, form a *group* called the **special orthogonal group of order 2**,\n",
    "denoted by $SO(2)$.\n",
    "All matrices $R \\in SO(2)$ have the following properties:\n",
    "1. Each column $c_i$ is a unit vector.\n",
    "2. The columns are orthogonal (i.e., $c_i \\cdot c_j = 0$ for $i \\neq j$).\n",
    "3. The inverse of $R$ is equal to its transpose, $R^{-1}=R^{T}$.\n",
    "4. The determinant of $R$ is +1.\n",
    "\n",
    "The first three properties hold for all orthogonal matrices.\n",
    "\n",
    "It is easy to understand the first two properties by thinking of the columns\n",
    "as representations of coordinate axes. We typically use unit vectors to represent\n",
    "the directions of coordinate axes, and any two coordinate axes are orthogonal to one\n",
    "another.\n",
    "\n",
    "The third property provides a simple method for inverting rotation matrices,\n",
    "which is a frequently used operation in robotics applications.\n",
    "\n",
    "The fourth property holds for rotation matrices, but not for arbitrary orthogonal\n",
    "matrices. \n",
    "The determinant of an arbitrary orthogonal matrix is $\\pm 1$.\n",
    "Those orthogonal matrices with determinant $+1$ correspond to right-handed coordinate systems\n",
    "(when curling the fingers of your right hand from the $x$-axis to the $y$-axis, your thumb points\n",
    "in the direction of the $z$-axis, which is \"out of the page\" in the 2D case).\n",
    "Those orthogonal matrices with determinant $-1$ correspond to left-handed systems,\n",
    "which are useful in computer graphics applications, but not so much for roboticists.\n",
    "Note that it is this fourth property that justifies the designation *special* for this\n",
    "set of matrices. \n",
    "\n",
    "As we will see in the next chapter, all of the properties described above generalize to the case\n",
    "of rotations in 3D, and the corresponding group of 3D rotation matrices, $SO(3)$.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Coordinate Transformations for Pure Rotations\n",
    "\n",
    "> The rotation group acts on 2D points.\n",
    "\n",
    "<figure id=\"fig:2drotation-trans\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/2d-rotation-transformation.png?raw=1\" style=\"width:10cm\" alt=\"\">\n",
    "<figcaption>Coordinate transformation under pure rotation.</figcaption>\n",
    "</figure>\n",
    "\n",
    "We can use rotation matrices to perform coordinate transformations under pure rotation.\n",
    "Consider a point $P$ that is rigidly attached to Frame 1 and, as illustrated in the Figures above, whose coordinates w.r.t. Frame 1\n",
    "are given by\n",
    "\n",
    "$$\n",
    "P_{}^{1}=\\begin{bmatrix}\n",
    "p_x \\\\\n",
    "p_y\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Using the definition of coordinates, we can describe the point $P$\n",
    "using the equation\n",
    "\n",
    "$$\n",
    "P = p_x x_1 + p_y y_1\n",
    "$$\n",
    "\n",
    "in which $x_1$ and $y_1$ are the vectors defining the coordinate axes of Frame 1."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To compute the coordinates $P^0$ of $P$ w.r.t. Frame 0, we merely project $P$ onto the $x$- and $y$- axes\n",
    "of Frame 0. Since the vectors $x_0$ and $y_0$ are unit vectors, this projection can again be accomplished\n",
    "using a the dot product:\n",
    "\n",
    "$$\n",
    "P^0 = \n",
    "\\begin{bmatrix}\n",
    "x_0 \\cdot P \\\\\n",
    "y_0 \\cdot P\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "x_0 \\cdot (p_x x_1 + p_y y_1) \\\\\n",
    "y_0 \\cdot (p_x x_1 + p_y y_1)\n",
    "\\end{bmatrix} \n",
    "=\n",
    "\\begin{bmatrix}\n",
    "(x_0 \\cdot x_1) p_x + (x_0 \\cdot y_1) p_y   \\\\\n",
    "(y_0 \\cdot x_1) p_x + (y_0 \\cdot y_1) p_y  \n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "x_0 \\cdot x_1  & x_0 \\cdot y_1   \\\\\n",
    "y_0 \\cdot x_1  & y_0 \\cdot y_1  \n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "p_x    \\\\\n",
    "p_y\n",
    "\\end{bmatrix}\n",
    "= R^0_1 P^1\n",
    "$$\n",
    "\n",
    "Summarizing the above, we have\n",
    "\n",
    "$$\n",
    "P^0 =  R^0_1 P^1\n",
    "$$\n",
    "\n",
    "which is how a rotation $R^0_1$ *acts* on a point $P^1$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} group\n",
    "```\n",
    "## The Group $SO(2)$ in GTSAM\n",
    "\n",
    "> `Rot2` is a group, and acts on `Point2`.\n",
    "\n",
    "$SO(2)$ is a *group*, in the mathematical sense. \n",
    "A **group** consists of a set (in our case, a set of 2D rotations) along\n",
    "with a group operator (in our case, composition).\n",
    "All groups posses certain properties, sometimes called the *group axioms*.\n",
    "For the group, $SO(2)$ these properties are as follows:\n",
    "\n",
    "1. *Closure*: For all rotations $R, R' \\in SO(2)$, their product is also in $SO(2)$, i.e., $RR' \\in SO(2)$.\n",
    "2. *Identity Element*: The $2\\times 2$ identity matrix $I$ is included in the group, and for\n",
    "all $R\\in SO(2)$ we have $RI = IR = R$.\n",
    "3. *Inverse*: For every $R \\in SO(2)$ there exists $R^{-1} \\in SO(2)$ such that $R^{-1}R = RR^{-1} = I$.\n",
    "4. *Associativity*: For all $R_1, R_2, R_3 \\in SO(2)$, $(R_1 R_2) R_3 = R_1 (R_2 R_3)$.\n",
    "\n",
    "In addition (only in 2D!) rotations in $SO(2)$ *commute*:\n",
    "\n",
    "5. *Commutativity*: For all $R_1, R_2 \\in SO(2)$, $R_1 R_2 = R_2 R_1$.\n",
    "\n",
    "\n",
    "It will not surprise anyone by now that all of this is built into GTSAM. In particular, the 2D rotations are represented by the type `Rot2`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.8660254 -0.5      ]\n",
      " [ 0.5        0.8660254]]\n"
     ]
    }
   ],
   "source": [
    "theta = math.radians(30)\n",
    "R = gtsam.Rot2(theta)\n",
    "print(R.matrix())\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Rotations in 2D form a *commutative* group, as demonstrated here in code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "True\n",
      "True\n",
      "True\n",
      "True\n",
      "True\n"
     ]
    }
   ],
   "source": [
    "print(isinstance(R * R, gtsam.Rot2)) # closure\n",
    "I2 = gtsam.Rot2.Identity()\n",
    "print(R.equals(R * I2, 1e-9)) # identity\n",
    "print((R * R.inverse()).equals(I2, 1e-9)) # inverse\n",
    "R1, R2, R3 = gtsam.Rot2(1), gtsam.Rot2(2), gtsam.Rot2(3)\n",
    "print(((R1 * R2)* R3).equals(R1 * (R2 * R3), 1e-9)) # associative\n",
    "print((R1 * R2).equals(R2 * R1, 1e-9)) # commutative\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, rotations can act on points, which we can do using matrix multiplication, or using the `Rot2.rotate` method:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "P0 = [2.73271005 4.18715844]\n",
      "P0 = [2.73271005 4.18715844]\n"
     ]
    }
   ],
   "source": [
    "R01 =  gtsam.Rot2(math.radians(20))\n",
    "P1 = gtsam.Point2(4,3)\n",
    "print(f\"P0 = {R01.matrix() @ P1}\")\n",
    "print(f\"P0 = {R01.rotate(P1)}\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} 2D rigid transforms, pose, transformation\n",
    "```\n",
    "## 2D Rigid Transforms aka SE(2)\n",
    "\n",
    "> From $SO(2)$ to $SE(2)$.\n",
    "\n",
    "**2D rigid transforms** combine rotation with translation, and are used in two *different* ways in robotics:\n",
    "\n",
    "\n",
    "* to specify the **pose** of a robot, which consists of a translation and an orientation\n",
    "\n",
    "* to talk about relative pose or the **transformation** between two different robot poses\n",
    "\n",
    "We can use the same mathematical object for both use cases, i.e., 2D rigid transforms $T\\in SE(2)$, which combine translation and orientation in a $3 \\times 3$ matrix, as explained below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<figure id=\"fig:2drotation-trans2\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/2d-coord-trans.png?raw=1\" style=\"width:20cm\" alt=\"\">\n",
    "<figcaption>Coordinate transformation under both rotation and translation.</figcaption>\n",
    "</figure>\n",
    "\n",
    "Let us start by assuming as before that the point $P$ is rigidly attached to a frame that is rotated\n",
    "by angle $\\theta$ w.r.t. Frame 0.\n",
    "If we now translate that frame according to a vector $d$, as shown in the figure above, the point $P$ undergoes\n",
    "the same translation as the frame.\n",
    "To calculate the coordinates of $P$ relative to Frame 0, we merely\n",
    "perform the rotational coordinate transformation as above,\n",
    "and then add the translation from the origin of Frame 0 to the origin of Frame 1:\n",
    "\n",
    "$$\n",
    "P^0 =  R^0_1 P^1 + d^0_1\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} special Euclidean group of order 2\n",
    "```\n",
    "We can write the above coordinate transformation as a matrix equation:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "P^0 \\\\ 1\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "R_{1}^{0} & d_{1}^{0}\\\\\n",
    "0_{2} & 1\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "P^1 \\\\ 1\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "in which $0_2$ denotes the row vector $[0~ 0]$.\n",
    "Note that the bottom row of this matrix equation is merely the equation\n",
    "\n",
    "$$\n",
    "1 = \\begin{bmatrix} 0 & 0 & 1 \\end{bmatrix} \n",
    "\\begin{bmatrix} p_x \\\\ p_y \\\\ 1 \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "which holds for any $p_x, p_y$.\n",
    "\n",
    "It is convenient to define the 3-vector $\\tilde{P}$ as\n",
    "\n",
    "$$\n",
    "\\tilde{P} =\n",
    "\\begin{bmatrix}\n",
    "P \\\\ 1\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "which we call the homogeneous coordinates of the point $P$. \n",
    "Using this convention, we can write the *homogeneous coordinate transformation* as\n",
    "\n",
    "$$\n",
    "\\tilde{P}^0 = T^0_1 \\tilde{P}^1\n",
    "$$\n",
    "\n",
    "\n",
    "The set of matrices of the form\n",
    "\n",
    "$$\n",
    "T =\n",
    "\\begin{bmatrix}\n",
    "R & d\\\\\n",
    "0_2 & 1\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "with $R \\in SO(2)$ and $d \\in \\mathbb{R}^2$ is called\n",
    "the **special Euclidean group of order 2**, denoted by SE(2)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Composition of Homogeneous Transformations\n",
    "\n",
    "> We can implement composition by matrix multiplication.\n",
    "\n",
    "<figure id=\"fig:TransformComposition\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/transform-composition.png?raw=1\" style=\"width:14cm\" alt=\"\">\n",
    "<figcaption>The composition of two homogeneous transformations. </figcaption>\n",
    "</figure>\n",
    "\n",
    "Consider a mobile robot that begins at some initial configuration, moves to a new location, reorients itself and then moves to another location. How can we determine the final configuration of the robot if we are given\n",
    "only the individual motions in the sequence?\n",
    "The answer is suprisingly simple.\n",
    "Suppose the point $P$ is rigidly attached to the robot.\n",
    "Using the coordinate transformation equation $\\tilde{P}^i = T^i_j \\tilde{P}^j$, we have the following:\n",
    "\n",
    "$$\\tilde{P}^0 = T^0_1 \\tilde{P}^1, ~~~~~ \\tilde{P}^1 = T^1_2 \\tilde{P}^2, ~~~~~ \\tilde{P}^0 = T^0_2 \\tilde{P}^2$$ \n",
    "\n",
    "Combining the first two of these gives\n",
    "\n",
    "$$\\tilde{P}^0 = T^0_1 T^1_2 \\tilde{P}^2$$\n",
    "\n",
    "and since this equation holds for every possible $\\tilde{P}^2$ we have\n",
    "\n",
    "$$T^0_2 = T^0_1 T^1_2 $$\n",
    "\n",
    "It is easy to show (by induction) that this result extends to an arbitarily long sequence of transformations.\n",
    "Therefore, if we have a sequence of robot configurations $T^{i-1}_i$, for $i = 1, \\dots , n$ we can compute\n",
    "the composite transformation $T^0_n$ using\n",
    "\n",
    "$$T^0_n = T^0_1 \\dots T^{n-1}_n $$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Group $SE(2)$ in GTSAM\n",
    "\n",
    "> `Pose2` is a *non-commutative* group, and also acts on `Point2`.\n",
    "\n",
    "Unsurprisingly, $SE(2)$ is also a group, as the following properties all hold:\n",
    "\n",
    "1. *Closure*: For all transforms $T, T' \\in SE(2)$, their product is also in $SE(2)$, i.e., $T T' \\in SE(2)$.\n",
    "2. *Identity Element*: The $3\\times 3$ identity matrix $I$ is included in the group, and for\n",
    "all $T \\in SE(2)$ we have $T I = I T = T$.\n",
    "3. *Inverse*: For every $T \\in SE(2)$ there exists $T^{-1} \\in SE(2)$ such that $T^{-1}T = T T^{-1} = I$.\n",
    "4. *Associativity*: For all $T_1, T_2, T_3 \\in SE(2)$, $(T_1 T_2) T_3 = T_1 (T_2 T_3)$.\n",
    "\n",
    "However, in contrast to 2D rotations, 2D rigid transforms do *not* commute. Also, The inverse $T^{-1}$ is not just the transpose; instead, we have:\n",
    "\n",
    "$$\n",
    "T^{-1} = \\begin{bmatrix}R & d\\\\0_2 & 1\\end{bmatrix}^{-1}\n",
    " = \\begin{bmatrix}R^T & -R^T d\\\\0_2 & 1\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "which you can easily verify by multiplying to obtain $I$. \n",
    "\n",
    "Again, all of this is built into GTSAM,where both 2D poses and 2D rigid transforms are represented by the type `Pose2`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2D Pose (x, y, theta) = (3, -2, 0.523599)\n",
      "corresponding to transformation matrix:\n",
      "[[ 0.8660254 -0.5        3.       ]\n",
      " [ 0.5        0.8660254 -2.       ]\n",
      " [ 0.         0.         1.       ]]\n"
     ]
    }
   ],
   "source": [
    "theta = math.radians(30)\n",
    "T = gtsam.Pose2(3, -2, theta)\n",
    "print(f\"2D Pose (x, y, theta) = {T}corresponding to transformation matrix:\\n{T.matrix()}\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2D transforms form a *non-commutative* group, as demonstrated here in code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "True\n",
      "True\n",
      "True\n",
      "True\n",
      "False\n"
     ]
    }
   ],
   "source": [
    "print(isinstance(T * T, gtsam.Pose2)) # closure\n",
    "I3 = gtsam.Pose2.Identity()\n",
    "print(T.equals(T * I3, 1e-9)) # identity\n",
    "print((T * T.inverse()).equals(I3, 1e-9)) # inverse\n",
    "T1, T2, T3 = gtsam.Pose2(1,2,3), gtsam.Pose2(4,5,6), gtsam.Pose2(7,8,9)\n",
    "print(((T1 * T2)* T3).equals(T1 * (T2 * T3), 1e-9)) # associative\n",
    "print((T1 * T2).equals(T2 * T1, 1e-9)) # NOT commutative\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, 2D transforms can act on points, which we can do using matrix multiplication, or using the `Pose2.transformFrom` method:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "P0 = [3.73271005 6.18715844 1.        ]\n",
      "P0 = [3.73271005 6.18715844]\n"
     ]
    }
   ],
   "source": [
    "T01 =  gtsam.Pose2(1, 2, math.radians(20))\n",
    "P1 = gtsam.Point2(4,3)\n",
    "print(f\"P0 = {T01.matrix() @ [4, 3, 1]}\")  # need to make P0 homogeneous\n",
    "print(f\"P0 = {T01.transformFrom(P1)}\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} manifold\n",
    "```\n",
    "## Lie Groups*\n",
    "\n",
    "Groups can be either discrete or continuous.\n",
    "For example, the set of integers along with addition as the group operation and zero as the identity forms a discrete\n",
    "group.\n",
    "In contrast, both $SO(2)$ and $SE(2)$ are continuous groups.\n",
    "Not only are they continuous, but they are examples of **manifolds**.\n",
    "Informally, a manifold is a set that is locally like $\\mathbb{R}^k$ for some $k$.\n",
    "In the present case, $SO(2)$ is locally like $\\mathbb{R}^1$, and $SE(2)$ is locally like $\\mathbb{R}^3$.\n",
    "\n",
    "While the topic of manifolds is beyond the scope of this book,\n",
    "the easiest way to understand this is to note that $SO(2)$ can be parameterized by the angle $\\theta \\in [0,2\\pi) \\subset \\mathbb{R}^1$.\n",
    "This parameterization is local (and not global) because it breaks down in a neighborhood of $\\theta = 2\\pi$ (as described above).\n",
    "This is not a problem, however, since in that case, we can simply\n",
    "choose to use $\\theta \\in [-\\pi, \\pi)$ instead of $\\theta \\in [0,2\\pi)$.\n",
    "We can completely *cover* $SO(2)$ using two parameterizations: one for cases when $\\theta$ is near $2\\pi$, \n",
    "and a second for when $\\theta$ is near $\\pi$.\n",
    "\n",
    "For the case of $SE(2)$, we merely use the two parameterizations $(x,y,\\theta) \\in \\mathbb{R}^2 \\times [0,2\\pi) \\subset \\mathbb{R}^3$\n",
    "and $(x,y,\\theta) \\in \\mathbb{R}^2 \\times [-\\pi,\\pi) \\subset \\mathbb{R}^3$,\n",
    "which demonstrates that *locally* $SE(2)$ is like $\\mathbb{R}^3$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```{index} Lie group\n",
    "```\n",
    "In addition to being manifolds, both $SO(2)$ and $SE(2)$ posses the additional property of being differentiable.\n",
    "Such groups are known as **Lie groups** (pronounced \"Lee groups\").\n",
    "While again beyond the scope of this book,\n",
    "we can understand this by considering the parameterizations for $SO(2)$ and $SE(2)$ described above.\n",
    "For each of these parameterizations, we can differentiate the elements of $SO(2)$ and $SE(2)$,\n",
    "which implies the existence of linear and angular velocities.\n",
    "In fact, in this chapter, we will define the actions of the car using these velocities as\n",
    "the inputs to the system.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GTSAM 101\n",
    "\n",
    "> A deeper dive in the GTSAM concepts used above.\n",
    "\n",
    "The types `Point2` and `Rot2` are used for 2D position and rotation.\n",
    "While `Point2` is really just an alias for a 3-dimensional numpy array, the `Rot2` type is a wrapper around a c++ GTSAM type that represents $SO(2)$ using just a cosine and sine value, i.e., a point on the unit circle."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using a 2D position and a 2D rotation we can create a 2D pose, represented by a `gtsam.Pose2`.As always, you can execute `help(gtsam.Pose2)` to get the full documentation of a class. Below is an excerpt with some useful methods. We have several constructors:\n",
    "\n",
    "```python\n",
    "__init__(...)\n",
    "    __init__(*args, **kwargs)\n",
    "    Overloaded function.\n",
    "    \n",
    "    1. __init__(self: Pose2) -> None\n",
    "    \n",
    "    2. __init__(self: Pose2, other: Pose2) -> None\n",
    "    \n",
    "    3. __init__(self: Pose2, x: float, y: float, theta: float) -> None\n",
    "    \n",
    "    4. __init__(self: Pose2, theta: float, t: Point2) -> None\n",
    "    \n",
    "    5. __init__(self: Pose2, r: Rot2, t: Point2) -> None\n",
    "    \n",
    "    6. __init__(self: Pose2, v: numpy.ndarray[numpy.float64[m, 1]]) -> None\n",
    "```\n",
    "\n",
    "where `t` above is just a `Point2`, and `v` is supposed to be a 3-vector with x, y, and $\\theta$, in that order. We also have `Pose2.transformFrom` and `Pose2.transformTo` methods, which act on 2D points:\n",
    "\n",
    "```python\n",
    " transformFrom(...)\n",
    "     transformFrom(*args, **kwargs)\n",
    "     Overloaded function.\n",
    "     \n",
    "     1. transformFrom(self: Pose2, point: Point2) -> Point2\n",
    "     ...\n",
    " |\n",
    " transformTo(...)\n",
    "     transformTo(*args, **kwargs)\n",
    "     Overloaded function.\n",
    "     \n",
    "     1. transformTo(self: Pose2, point: Point2) -> Point2\n",
    "     ....\n",
    "```\n",
    "\n",
    "Both have them have three overloads, but we show only the most commonly used one above. When using code, it is often a good idea to use indices to keep the different coordinate frames apart. For example, for a pose $T^a_b$ the method `transformFrom` implements the following \n",
    "\n",
    "$$\n",
    "P^a = T^a_b P^b\n",
    "$$\n",
    "\n",
    "i.e., it transforms *from* frame B to frame A. The other method, `transformTo` transforms *to* frame B and hence implements\n",
    "\n",
    "$$\n",
    "P^b = (T^a_b)^{-1} P^a\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S11_sorter_state.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "gtbook",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12 | packaged by conda-forge | (default, Oct 12 2021, 21:21:17) \n[Clang 11.1.0 ]"
  },
  "latex_metadata": {
   "affiliation": "Georgia Institute of Technology",
   "author": "Frank Dellaert and Seth Hutchinson",
   "title": "Introduction to Robotics"
  },
  "vscode": {
   "interpreter": {
    "hash": "9f7376ced4243bb13dfcffa8a3ba834e0602aa8334cd3a1d8ba8d285f4628083"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}