7_hw_cp220_Q.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "view-in-github"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/AshrithSagar/CP220-M4RAS-2024/blob/main/7_hw_cp220_Q.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ZwS1qFqSIoRl"
      },
      "source": [
        "---\n",
        "**Lab7**\n",
        "\n",
        "Projective Geometry <br>\n",
        "Euclidean Transforms <br>\n",
        "Lie Theory <br>\n",
        "\n",
        "*References* <br>\n",
        "Main reference: Micro Lie Theory for State Estimation in Robotics, Joan Sola, et. al, https://arxiv.org/abs/1812.01537\n",
        "\n",
        "Second reference: Lie. groups for 2D and 3D tranformations, Ethan Eade, https://www.ethaneade.com/lie.pdf\n",
        "\n",
        "Third reference: Physics from Symmetry, Chapter 3, Jakob Swhitchenberg. https://core.ac.uk/download/pdf/326762666.pdf\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "LtQ1NA6T0wBY"
      },
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "import matplotlib.pyplot as plt\n",
        "import cv2"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "as10BuvIQ7UM"
      },
      "source": [
        "---\n",
        "**Projective 2D Plane $ℙ^2$**\n",
        "\n",
        "A point P in Euclidean 2D plane is represented as $(x_1,x_2)$\n",
        "\n",
        "A line is the set of points $l: \\{(x_1, x_2)| ax_1 + bx_2 + c = 0\\}$\n",
        "\n",
        "In homogenous coordinate system, we add an extra dimension to represent a point and normal points have a non-zero third component.\n",
        "\n",
        "Thus the same point is represented as $X=\\begin{bmatrix} x_1 \\\\ x_2 \\\\ 1 \\end{bmatrix}$\n",
        "\n",
        "If we also consider  any scaled multiple of the 3D vector is taken to represent the same point, then that is mathematical described as the projective 2D plane $ℙ^2$.\n",
        "\n",
        "Thus the representation is: $X=k\\begin{bmatrix} x_1 \\\\ x_2 \\\\ 1 \\end{bmatrix}$ with $k$ any non-zero real number.\n",
        "\n",
        "Similary the line in 2D plane using the language of projective geometry is the subspace : $l=k\\begin{bmatrix} a \\\\ b \\\\ c \\end{bmatrix}$ for $k \\in ℝ$.\n",
        "\n",
        "With this representation, we can see that a point $X$ lies on a line $l$ iff $l^TX = 0$\n",
        "\n",
        "*Intersection of two lines*\n",
        "\n",
        "We know that two non-parallel lines $l, m$ intersect at a point. Let that point be $x$.\n",
        "\n",
        "Then we can calculate that point as $x = l \\times m$\n",
        "\n",
        "Note that $l^T (l \\times m ) = m^T (l \\times m) = 0$ and hence $x$ lies on both lines and hence is the intersection point.\n",
        "\n",
        "\n",
        "Similarly, two points define a line. $x$, $y$ are the two points then\n",
        "\n",
        "$l=x \\times y$ is the line made by the two points."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "7l-_IaE-UI0j"
      },
      "source": [
        "---\n",
        "Consider two parallel lines $l=\\begin{bmatrix} a \\\\ b \\\\ c\\end{bmatrix}$ and $m=\\begin{bmatrix} a \\\\ b \\\\ c^{'} \\end{bmatrix}$\n",
        "\n",
        "Then their intersection point is $p=l \\times m = (c-c^{'})\\begin{bmatrix} -b \\\\ a  \\\\ 0 \\end{bmatrix}$\n",
        "\n",
        "Thus we can also think of $p$ as a point - but at infinity, in the direction (-b, a), which is the direction along the line.\n",
        "\n",
        "The augmentation of the 2D plane with points at infinity is called the 2D projective plane $ℙ^2$\n",
        "\n",
        "\n",
        "Note that all points at infinity lie on the line $l=\\begin{bmatrix} 0 \\\\ 0 \\\\ 1\\end{bmatrix}$. Hence this line is called the line at infinity."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "N_xIEFJ4WB44"
      },
      "source": [
        "---\n",
        "*Projective Geometry*\n",
        "\n",
        "Consider a vector space $ℜ^{n+1}$\n",
        "\n",
        "Consider its susbaces of dimensions 1, 2, 3,...n.\n",
        "\n",
        "Under projective geometry,\n",
        "\n",
        "Every point is mapped to a vector subspace of dimension 1, a line to subspace of dimenson 2, a plane to a subspace of dimension 3 etc.\n",
        "\n",
        "As an example, consider $ℜ^4$.\n",
        "\n",
        "Here  any subspace of dimension 1 is written as\n",
        "$\\{k\\begin{bmatrix} x_1 \\\\ x_2 \\\\ x_3 \\\\ x_4 \\end{bmatrix}: for\\ some\\  x_1, x_2, x_3, x_4 \\&\\forall k \\in ℜ \\} = P$ where P is some point in 3D euclidean space.\n",
        "\n",
        "More specifically, in homogeonous coordinates, we can write the point P as $P=\\begin{bmatrix} x_1/x_4 \\\\ x_2/x_4 \\\\ x_3 / x_4 \\\\ 1 \\end{bmatrix}$ provided $x_4 \\ne 0$\n",
        "\n",
        "if $x_4 = 0$ then $P=\\begin{bmatrix} x_1 \\\\ x_2 \\\\ x_3 \\\\ 0 \\end{bmatrix}$ is still a point in the geometry - except it is called a point at infinity (or an ideal point).\n",
        "\n",
        "A 3-dimensional subspace is used to represent a plane as\n",
        "\n",
        "$\\pi = \\begin{bmatrix} \\pi_1 \\\\ \\pi_2 \\\\ \\pi_3 \\\\ \\pi_4 \\end{bmatrix}$. Note again, any scalar multiple will refer to the same plane.\n",
        "\n",
        "Since 3 non-collinear  points define a plane, we have for points P, Q, R, a plane $\\pi$ which goes through them as\n",
        "\n",
        "$\\begin{bmatrix} P^T \\\\ Q^T \\\\ R^T \\end{bmatrix}\\pi = 0$\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "9o0NkLqod_te"
      },
      "source": [
        "---\n",
        "**Q1**\n",
        "\n",
        "Let\n",
        "\n",
        "$P=\\begin{bmatrix} p \\\\ 1\\end{bmatrix}$\n",
        "\n",
        "$Q=\\begin{bmatrix} q \\\\ 1\\end{bmatrix}$\n",
        "\n",
        "$R=\\begin{bmatrix} r \\\\ 1\\end{bmatrix}$\n",
        "\n",
        "where $p,q,r$ are inhomogenous 3D coordinates.\n",
        "\n",
        "Find the normal of the plane formed by points P,Q,R (without doing SVD)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nbq5-a4keLs2"
      },
      "source": [
        "Ans:\n",
        "\n",
        "\n",
        "\n",
        "Normal is :"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Kxv75lg7eT5i",
        "outputId": "1a9cdd68-8e3c-4403-9e53-159b64933a38"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Unit normal to plane formed by P,Q,R is: [-0.57735027 -0.57735027 -0.57735027]\n"
          ]
        }
      ],
      "source": [
        "P = np.array([1, 0, 0, 1]).reshape((4, 1))\n",
        "Q = np.array([0, 1, 0, 1]).reshape((4, 1))\n",
        "R = np.array([0, 0, 1, 1]).reshape((4, 1))\n",
        "\n",
        "PQ = Q[:3] - P[:3]\n",
        "RP = P[:3] - R[:3]\n",
        "pi = np.cross(PQ.flatten(), RP.flatten())\n",
        "\n",
        "print(f\"Unit normal to plane formed by P,Q,R is: {pi/np.linalg.norm(pi)}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eni-2WDlRqCY"
      },
      "source": [
        "---\n",
        "**Q2**\n",
        "\n",
        "Fill in the code to\n",
        "a) find plane through three points P,Q,R. return the plane such that its normal vector has unit magnitude\n",
        "\n",
        "b) find the distance between a point P and the plane q. Also return the point on plane q that is closest to P\n",
        "\n",
        "c) find the distance between a Point P and the line formed by two points P1, P2. Also return the point on line closest to P"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "0YQrrg1EaiBY"
      },
      "outputs": [],
      "source": [
        "def find_plane(P, Q, R):\n",
        "    \"\"\"find the plane formed by points P,Q,R\n",
        "    return as a normalized plane\n",
        "    \"\"\"\n",
        "    PQ = Q[:3] - P[:3]\n",
        "    RP = P[:3] - R[:3]\n",
        "    pi = np.cross(PQ.flatten(), RP.flatten())\n",
        "    pi = pi / np.linalg.norm(pi)\n",
        "    A, B, C = pi\n",
        "    D = -np.dot(pi, P[:3])[0]\n",
        "    return np.array([[A], [B], [C], [D]])\n",
        "\n",
        "\n",
        "def dpq(P, q):\n",
        "    \"\"\"return distance of point P to plane q and closest point on q\"\"\"\n",
        "    A, B, C, D = q.flatten()\n",
        "    distance = (A * P[0] + B * P[1] + C * P[2] + D) / np.sqrt(A**2 + B**2 + C**2)\n",
        "    point_on_plane = P[:3] - distance * np.array([A, B, C]) / np.sqrt(\n",
        "        A**2 + B**2 + C**2\n",
        "    )\n",
        "    return distance, point_on_plane.reshape(3, 1)\n",
        "\n",
        "\n",
        "def dpl(P, P1, P2):\n",
        "    \"\"\"find distance of P to line formed by P1 & P2\"\"\"\n",
        "    line_dir = P2[:3] - P1[:3]\n",
        "    P1_to_P = P[:3] - P1[:3]\n",
        "    t = np.dot(P1_to_P.flatten(), line_dir.flatten()) / np.dot(\n",
        "        line_dir.flatten(), line_dir.flatten()\n",
        "    )\n",
        "    closest_point_on_line = P1[:3] + t * line_dir\n",
        "    distance = np.linalg.norm(P[:3] - closest_point_on_line)\n",
        "    return distance, closest_point_on_line.reshape(3, 1)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "M2LG1pLeIWp1",
        "outputId": "561a9d23-af0b-44a6-aeb1-6a069bc49472"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "The plane passing through P,Q,R is: pi=[[-0.57735027]\n",
            " [-0.57735027]\n",
            " [-0.57735027]\n",
            " [ 0.57735027]]\n"
          ]
        }
      ],
      "source": [
        "pi = find_plane(P,Q,R)\n",
        "\n",
        "print(f\"The plane passing through P,Q,R is: pi={pi}\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "HzRqmTt2Ju-k",
        "outputId": "6dcd2cd5-9d58-4550-f1bc-f5a9fcc266a3"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "(array([[-1.]]),\n",
              " array([[1.],\n",
              "        [1.],\n",
              "        [0.],\n",
              "        [1.]]))"
            ]
          },
          "execution_count": 44,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "q = np.array([[0], [1], [0], [-1]])\n",
        "dpq(P, q)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "l-V2QbMLOXy1",
        "outputId": "ef12557c-37f7-4f76-dac6-e325fdeb121f"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "(array([[-1.22474487]]),\n",
              " array([[2.22044605e-16],\n",
              "        [5.00000000e-01],\n",
              "        [5.00000000e-01],\n",
              "        [1.00000000e+00]]))"
            ]
          },
          "execution_count": 45,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "dpl(P,Q,R)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "dGnt6PBzhCks"
      },
      "source": [
        "---\n",
        "Any point on the plane $\\pi=\\begin{bmatrix} a \\\\b \\\\c \\\\d \\end{bmatrix}$ can be written in a parametric form as follows.\n",
        "\n",
        "Consider the matrix:\n",
        "\n",
        "$M=\\begin{bmatrix} -b/1 & 1 & 0 & 0 \\\\ -c/d & 0 & 1 & 0 \\\\ -d/a & 0 & 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "We can check that $M \\pi = 0$\n",
        "Thus any linear combination of rows is also orthogonal to $\\pi$ and hence a parametric represention of any point on the plane is:\n",
        "\n",
        "$P_\\pi(\\lambda_1, \\lambda_2) = \\lambda_1\\begin{bmatrix} -b/a \\\\1 \\\\0 \\\\0 \\end{bmatrix} + \\lambda_2 \\begin{bmatrix} -c/a \\\\ 0 \\\\1 \\\\0 \\end{bmatrix}+ \\begin{bmatrix}-d/a \\\\ 0 \\\\ 0 \\\\ 1 \\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "WAK8g7kqtAFd"
      },
      "source": [
        "---\n",
        "**Line in 3D:**\n",
        "\n",
        "*Null space representation*\n",
        "\n",
        "A line is obtained by the \"joining two points\" P, Q.\n",
        "\n",
        "In projective geometry, P, Q are 1-dimensional subspaces and their join is the span of the two subspaces.\n",
        "\n",
        "i.e line $l = span(P,Q)$\n",
        "\n",
        "If $W = \\begin{bmatrix} P^T \\\\ Q^T \\end{bmatrix}$ then line $l$ is the row span of W.\n",
        "\n",
        "The line is also the intersection of two planes $\\pi_1, \\pi_2$ which pass through the points $P, Q$\n",
        "\n",
        "Let $W^* = \\begin{bmatrix} \\pi_1 & \\pi_2\\end{bmatrix}$ then\n",
        "\n",
        "$WW^* = 0_{2\\times 2}$ since P,Q lie on the planes.\n",
        "\n",
        "Hence the line is also represented as column space of $W^*$ - the pencil of planes through l."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "lc94oND4u2xd"
      },
      "source": [
        "---\n",
        "*Plucker Matrix representation*\n",
        "\n",
        "Let A, B be points (in homogenous coordinates)\n",
        "\n",
        "Then,\n",
        "\n",
        "$l: L = AB^T - BA^T$ is a 4x4 skew symmetric matrix of rank 2.\n",
        "\n",
        "Note if $A, B \\in \\pi_1, \\pi_2$ the two planes, then\n",
        "\n",
        "$L\\pi_1 = L\\pi_2 = 0$\n",
        "\n",
        "Thus the 2D null space of $L$ is the pencil of planes with l as the spine.\n",
        "\n",
        "The dual representation of the line l is $l: L^* = \\pi_1 \\pi_2^T - \\pi_2 \\pi_1^T$\n",
        "\n",
        "The plane formed by joining a point P with line $l$ is given as\n",
        "\n",
        "$\\pi = L^*P$\n",
        "\n",
        "Similarly the point of intersection of line $l$ with plane $\\pi_3$ is\n",
        "\n",
        "$Q = L\\pi_3$\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "mzYgryPgwXhN"
      },
      "source": [
        "---\n",
        "*Plucker coordinate representation*\n",
        "\n",
        "The plucker coordinates are the six non-zero elements of the 4x4 plucker matrix L\n",
        "\n",
        "$L = \\{l_{12}, l_{12}, l_{14}, l_{23}, l_{42}, l_{34}\\}$\n",
        "\n",
        "This is a homogenous 6-vector\n",
        "\n",
        "Since $det(L)=0$, we have:\n",
        "\n",
        "$l_{12}l_{34}+l_{13}l_{42}+l_{14}l_{23} = 0$\n",
        "\n",
        "Thus not all 6-vectors are lines - they also need to satisfy the above equation.\n",
        "\n",
        "Four points P, Q, R, S are co-planar (i.e. the lines PQ, RS intersect) iff\n",
        "\n",
        "$det([P,Q,R,S]) = 0$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Ax6UnB8NGvlX"
      },
      "source": [
        "---\n",
        "**Q3**\n",
        "\n",
        "A drone is flying in a straight trajectory formed by points P1, P2.\n",
        "\n",
        "A window is located  with vertices at points P,Q,R,S.\n",
        "\n",
        "Determine if the drone will pass through the window (assume drone has 0 size)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "CE-5DtbZGd4B",
        "outputId": "026690f2-49c0-4eb9-d102-dd1fb8436bbb"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "U=[[ 5.00000000e-01]\n",
            " [-1.66533454e-16]\n",
            " [ 5.00000000e-01]\n",
            " [ 1.50000000e+00]]\n",
            "Drone passes through window\n"
          ]
        }
      ],
      "source": [
        "P = np.array([0,0,0,1]).reshape(4,1)\n",
        "Q = np.array([0,0,1,1]).reshape(4,1)\n",
        "R = np.array([1,0,0,1]).reshape(4,1)\n",
        "S = np.array([1,0,1,1]).reshape(4,1)\n",
        "\n",
        "P1 = np.array([0.5,0.5,0,1]).reshape(4,1)\n",
        "P2 = np.array([0,-1,1,1]).reshape(4,1)\n",
        "\n",
        "#<Your code here>\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "BlZfgcgz3PV3"
      },
      "source": [
        "---\n",
        "**Transforms as groups**\n",
        "\n",
        "Groups naturally arise as a transformations over some set onto itself, that preserves some property as an invariance. The composition of transformations naturally defines a binary operator.\n",
        "\n",
        "\n",
        "For example, the set of rotation matrices, when they act on a vector space, preserves the lengths and relative angles of vectors in the vector space - and hence that is the invariance. The set of rotations form a group and the group operator is the matrix multiplication.\n",
        "\n",
        "More formally, a group is an set with a product operation, an inverse for each element and a special element which is the identity, with the following properties:\n",
        "\n",
        "Let the  group $G$ consiste of elements $\\{g,h,k,...\\}$ and an operator $\\cdot$, a\n",
        "\n",
        "a) *Closure*:   $g\\cdot h \\in G \\forall g,h \\in G$.\n",
        "\n",
        "\n",
        "b) *Identity*: There is an element $e \\in G$ such that $g \\cdot e = e \\cdot g = g \\forall g \\in G$\n",
        "\n",
        "c) *Inverse*: Every element has an inverse, i.e. $\\forall g \\in F, ∃ g^{-1}, s.t. , g \\cdot g^{-1} = g^{-1} \\cdot g = e$\n",
        "\n",
        "d) *Associativity*: The group operator is associative, i.e. $g \\cdot (h \\cdot k) = (g \\cdot h) \\cdot k$\n",
        "\n",
        "\n",
        "Let $V$ be the set on which the elements of $G$ act on. i.e. $\\forall g \\in G, g:V → V$ such that $g(v)  \\in V$ and $(g \\cdot h) ( v) = g (h (v))$\n",
        "\n",
        "\n",
        "Example 2: The set of transforms that shuffles a card deck, while preserving the total number of cards is a permutation group operation. This is a finite group with $n!$ elements.\n",
        "\n",
        "Example 3: Let V be a vector space over $ℝ$, then the set of isomorphisms $T = \\{t|t:V→V\\}$ is a continuous group.\n",
        "\n",
        "For a finite-dimensional vector space,each transform can be represented as a  $n\\times n$ non-singular matrix.\n",
        "This group is also called general linear group : $GL(n)$\n",
        "\n",
        "The set of all orthogonal matrices is a sub-group of $GL(n)$: $O(n)$. Determinant is $\\pm 1$\n",
        "\n",
        "The set of all rotation matrices is a sub-group of orthogonal group (as its determinant is +1): $SO(n)$\n",
        "\n",
        "the special Euclidean group $SE(3)$ refers to 3D rigid body motions - includes rotation and translation."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "_I5j_io94jZb"
      },
      "source": [
        "---\n",
        "**Group of Rotations in 2D**\n",
        "\n",
        "Rotations in 2D can be obtained by two different ways:\n",
        "\n",
        "*Complex Numbers*\n",
        "We can represent the 2D vector space $ℝ^2$  as a complex number $v=a+ib \\in ℂ $ with $a,b \\in ℝ$ and $i=\\sqrt{-1}$\n",
        "\n",
        "Then a rotation transformation of $ℂ$ that preserves the magnitude of the complex number can be achived by a complex multiplication with a complex number of unit magnitude. $z^*z = |z|^2=1$ and $z:ℂ→ℂ$ is defined as $z(v)=zv$, the complex number multiplication.  $(zv)^*(zv) = z*v*zv = v*v=|v|^2$ and preserves length.\n",
        "\n",
        "The set $SU(1)=\\{z|z \\in ℂ\\ \\&\\ |z|^2=1\\}$ is the Special Unitary group of dimestion 1 and represents rotations. The group elements can be visualized as the unit circle $S^1$ in the complex plane. Elements of SU(1) can also be represented as $z = e^{i\\theta}$\n",
        "\n",
        "We can see that the elements of the group are generated by $i\\theta$ via an exponentiation and is called the generator\n",
        "\n",
        "*2D rotation matrices*\n",
        "If we represent the 2D vector space $ℝ^2$ as a vectors $v=\\begin{bmatrix} v_1 \\\\ v_2 \\end{bmatrix}$ then the rotation transform of this vector space can also be represented as 2D orthogonal matrix matrix with determinant 1 i.e\n",
        "\n",
        "$R(\\theta)=\\begin{bmatrix} cos(\\theta) & -sin(\\theta) \\\\ sin(\\theta) & cos(\\theta) \\end{bmatrix}$ and $SO(2) = \\{R| R^TR=I \\& |R|=1\\}$ is the group.\n",
        "\n",
        "Note that $SU(1)$ is isomorphic to $SO(2)$ - i.e we can find a bijective mapping\n",
        "\n",
        "$\\pi: SU(1) → SO(2)$ such that\n",
        "\n",
        "$\\pi(z_1z_2) = \\pi(z_1)\\pi(z_2)$ for all $z_1, z_2 \\in SU(2)$\n",
        "\n",
        "If $z = e^{i\\theta}$ then one such mapping is:\n",
        "\n",
        "$\\pi(z) = \\begin{bmatrix} cos(\\theta) & -sin(\\theta) \\\\ sin(\\theta) & cos(theta) \\end{bmatrix}$\n",
        "\n",
        "Thus we can map $i$ to $\\begin{bmatrix} 0 & -1 \\\\ 1 & 0 \\end{bmatrix}$ and $1$ to $\\begin{bmatrix} 1 & 0 \\\\ 0 & 1 \\end{bmatrix}$.\n",
        "\n",
        "Hence any complex number $z=a+ib$ gets mapped to $a\\begin{bmatrix} 0 & 1 \\\\ -1 & 0 \\end{bmatrix}+b\\begin{bmatrix} 1 & 0 \\\\ 0 & 1 \\end{bmatrix}$ and can also be represented as $2 \\times 2$ real matrices.\n",
        "\n",
        "In addition, we can also note the following: for any real $\\theta$, let $\\hat{\\theta} = \\begin{bmatrix} 0 & -\\theta \\\\ \\theta & 1 \\end{bmatrix}$, then\n",
        "\n",
        "$R(\\theta) = e^{\\hat{\\theta}}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "IyJxksmhAjCM"
      },
      "source": [
        "---\n",
        "**Group of Rotations in 3D**\n",
        "\n",
        "We can rotate about each of the three axes, $x, y, z$ to get a rotation in 3D. The following represent counter clock wise (CCW) rotation (when looked down from the the tip of the axis), about the z, x, y axis respectively by an angle of $\\theta$.\n",
        "\n",
        "$R_z = \\begin{bmatrix} cos(\\theta) & -sin(\\theta) & 0 \\\\ sin(\\theta) & cos(\\theta) & 0 \\\\ 0 & 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "$R_x = \\begin{bmatrix} 1 & 0 & 0 \\\\0 & cos(\\theta) & -sin(\\theta) \\\\ 0 & sin(\\theta) & cos(\\theta)  \\end{bmatrix}$\n",
        "\n",
        "$R_y = \\begin{bmatrix}cos(\\theta) & 0 &  sin(\\theta) \\\\ 0 & 1 & 0 \\\\  -sin(\\theta) & 0 & cos(\\theta)  \\end{bmatrix}$\n",
        "\n",
        "\n",
        "Any rotation matrix can then be composed as $R=R_x(\\theta)R_y(\\phi)R_z(\\gamma)$ - where the angles, $\\theta, \\phi, \\gamma$ are called the Euler angles (CCW is positive).\n",
        "\n",
        "The set $SO(3) = \\{R| R^TR=RR^T=I\\ \\& \\ |R|=1\\}$ with matrix multiplication as the operator is a  group of 3D rotations - called the special orthogonal group.\n",
        "\n",
        "\n",
        "Any rotation can also be thought of as a CCW rotation about some unit axis )\"axis-angle\" representation).\n",
        "\n",
        "If $\\theta$ is angle of CCW rotation about the unit axis $\\omega=\\begin{bmatrix} \\omega_1 \\\\ \\omega_2 \\\\ \\omega_3 \\end{bmatrix}$  (CCW is with looking down into the axis), and $||\\omega||=1$, then the corresponding skew symmetric matrix:\n",
        "\n",
        "$\\omega_\\times = \\begin{bmatrix}0 & -\\omega_3 & \\omega_2 \\\\ \\omega_3 & 0 & -\\omega_1 \\\\ -\\omega_2 & \\omega_1 & 0 \\end{bmatrix}$\n",
        "\n",
        "and we can write $R$ as:\n",
        "\n",
        "$R = e^{\\theta \\omega_\\times} = I + \\theta \\omega_\\times + \\frac{\\theta^2 \\omega_x^2}{2!}+...$\n",
        "\n",
        "You can check by multiplying out $\\omega_\\times^3 = -\\omega_\\times$. Using this in the above series expansion and consolidating terms we can show that:\n",
        "\n",
        "$R = e^{\\theta \\omega_\\times} = I + sin(\\theta)\\omega_\\times + (1-cos(\\theta))\\omega_\\times^2$ which is the well know Rodrigues formula to get the rotation matrix from the axis angle representation.\n",
        "\n",
        "To go in the other direction, since $R^T = R^{-1} = e^{-\\theta \\omega_\\times}$, we get:\n",
        "\n",
        "$\\omega_\\times = \\frac{R-R^T}{2sin(\\theta)}$\n",
        "\n",
        "$\\theta = arccos \\frac{tr(R)-1}{2}$\n",
        "\n",
        "and $log(R) = \\theta\\omega_x = \\frac{\\theta}{2sin(\\theta)} (R-R^T)$\n",
        "\n",
        "If $\\theta $ is very small, then we can write $\\frac{\\theta}{sin(\\theta)} = \\frac{1}{1-\\frac{\\theta^2}{6}+...} \\approx 1+\\frac{\\theta^2}{6}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ctyQvSSRzFQ9"
      },
      "source": [
        "---\n",
        "**Quaternions**\n",
        "\n",
        "Analogous to 2D complex numbers, we can use 4D complex numbers - also called **Quaternions** to find an alternate representation of rotation transformation of 3D space. The space of quaternions is denoted as\n",
        "\n",
        "$ℍ := \\{q = q_0 + q_1 i + q_2 j + q_3 k\\}$\n",
        "\n",
        "Where we defined $i^2 = j^2 = k^2 = -1$ and $ijk = -1$.\n",
        "\n",
        "Therefore $ij = k$, $jk = i$ and $ki=j$\n",
        "\n",
        "$ℍ$ is a vector space over $ℝ$\n",
        "\n",
        "We can defined quaternion multiplication\n",
        "\n",
        "$q r = (q_0 + q_1 i + q_2 j + q_3 k)*(r_0 + r_1 i + r_2 j + r_3 k)$ which is also a quaternion and can be easily computed using the rules for multiplying i,j,k.\n",
        "\n",
        "Multiplication of quaternions is distributive over addition.\n",
        "\n",
        "Such a space with the quaternion multiplication is closed under multiplication and with this, $ℍ$ is also called quaternion algebra.\n",
        "\n",
        "Conjugate of a quaternion is defined as: $q^* = q_0 - q_1 i - q_2 j - q_3 k$ and hence $q^*q = qq^* = |q|^2 = q_0^2+q_1^2+q_2^2+q_3^2$ represents its magnitude.\n",
        "\n",
        "For every non-zero quaternion $q$, we can find its inverse as $q^{-1}=q^*/|q|^2$\n",
        "\n",
        "Thus $\\mathbb{H}\\backslash 0$ (set of non-zero quaternions) is a group.\n",
        "\n",
        "We can represent any Euclidean 3D vectors ($ℝ^3$) as pure quaternions $v = v_1 i + v_2 j + v_3k$ (i.e. with its scalar component 0).\n",
        "\n",
        "The action of $q$ on a vector $v$ to give another vector $v^{'}$ is defined as\n",
        "\n",
        "$v^{'} = q v q^*$\n",
        "\n",
        "A *unit quaternion* $q$ has its magnitude 1,\n",
        "\n",
        "i.e. $|q|^2=qq^*=1$).\n",
        "\n",
        "It can also be written as\n",
        "\n",
        "$q(\\theta,\\bf{v})$ $=cos(\\theta)+sin(\\theta)(v_1 i+ v_2 j + v_3 j)$ where $\\bf{v}$ $= \\begin{bmatrix} v_1 \\\\  v_2 \\\\ v_3\\end{bmatrix}$ is a unit vector and $\\theta$ is the CCW rotation about it (when looking down into the vector $\\bf{v}$ from its tip).\n",
        "\n",
        "When we rotate any arbitrary vector $u$ with a unit quaternion $q(\\theta/2,\\bf{v})$, the rotated vector is given as:\n",
        "\n",
        "$u^r = q(\\theta/2,v)uq(\\theta/2,v)^*$ and the $u^r$ is rotated CCW by angle $\\theta$ about the axis $\\bf{v}$\n",
        "\n",
        "We can check that\n",
        "\n",
        "$|u^r|^2=u^ru^{r*} = (q u q^*)(quq^*)^* = quq^*qu^*q^* = |u|^2qq^* = |u|^2$\n",
        "\n",
        "Thus length is preserved under this transformation - as is expected of rotations.\n",
        "\n",
        "Moreover $u_r = q(\\pi+\\theta/2,v)uq(\\pi+\\theta/2,v)^*$ rotates by $\\pi+\\theta=\\theta$\n",
        "\n",
        "Thus for every rotation by $\\theta$ given by a matrix in $SO(3)$, there are two different quaternions which give the same rotation. Hence the set of unit quaternions (SU(2) - see below) double covers SO(3).\n",
        "\n",
        "\n",
        "Consider the set of complex unitary matrices $SU(2) = \\{u_{2\\times 2}|u=\\begin{bmatrix} q_0+iq_3 & q_1+iq_2 \\\\ -q_1+iq_2 & q_0-iq_3 \\end{bmatrix}\\}$ where $q = q_0 + iq_1 + jq_2 +kq_3$ is a unit quaternion.\n",
        "\n",
        "We can easily check that $u^{*T}u = I$ and $det(u) = 1$. Also given a $2\\times 2$ unitary matrix, we can find a matching unit quaternion. Hence\n",
        "SU(2) is isomorphic to the sub-algebra of unit quaternions.\n",
        "\n",
        "This set geometrically is $S^3$ surface of a sphere of unit radius in 4D space (hard to visualize). Analogy is the unit circle for SU(1).\n",
        "\n",
        "Thus any 3D vector $u$ can also be written as a 2x2 matrix : $u=\\begin{bmatrix} u_3i & u_1+u_2i \\\\ -u_1+u_2i & -u_3i\\end{bmatrix}$\n",
        "\n",
        "and $u^r = q(\\frac{\\theta}{2}) u q(\\frac{\\theta}{2})^*$ rotates by $\\theta$ and calculated via matrix  multiplication\n",
        "\n",
        "Note $q(\\theta/2)$ and $q(\\pi + \\theta/2)$ both achieve a rotation of $\\theta$, i.e. map to the same rotation matrix $R(\\theta)$.\n",
        "\n",
        "Hence SU(2) is said to be a double cover of $SO(3)$\n",
        "\n",
        "*Exponential and logarithms*\n",
        "\n",
        "For a unit quaternion $q(\\theta,\\bf{v})$, where $v$ is unit vector (pure quaternion), we can use the formal expansion of the exponential function and write:\n",
        "\n",
        "$q = e^{\\theta\\bf{v}}$ $ = 1 + \\theta\\bf{v}$ $+\\frac{1}{2!}\\theta^2\\bf{v}^2$ $+\\frac{1}{3!}\\theta^3\\bf{v}^3$ $+... = cos(\\theta) + sin(\\theta)\\bf{v}$  \n",
        "\n",
        "by applying the quaternion multiplication rules and using the Taylor series expansion of $cos$ and $sin$.\n",
        "\n",
        "and given $q=q_0+iq_1+jq_2+kq_3$ a unit quaternion\n",
        "\n",
        "$log(q) = \\theta \\bf{v}$\n",
        "\n",
        "where $\\theta = arccos(q_0)$\n",
        "\n",
        "$\\bf{v}$ $=1/sin(\\theta)\\begin{bmatrix}q_1\\\\q_2\\\\q_3\\end{bmatrix}$ when $sin(\\theta) \\ne 0$ and 0 otherwise."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "00g8oqPrcoCm"
      },
      "outputs": [],
      "source": [
        "def ToSU2(v, theta=np.pi/2):\n",
        "  '''Convert to SU(2) 2x2 representation a quaternion given as\n",
        "   cos(theta) + sin(theta)unit. v is a unit norm vector.\n",
        "   For vectors, which are pure quaternions, theta=np.pi/2'''\n",
        "  q = np.zeros((2,2),dtype=complex)\n",
        "  #u = v/np.linalg.norm(v)\n",
        "  q[0,0] = complex(np.cos(theta), np.sin(theta)*v[2])\n",
        "  q[0,1] = complex(np.sin(theta)*v[0],np.sin(theta)*v[1])\n",
        "  q[1,0] = complex(-np.sin(theta)*v[0],np.sin(theta)*v[1])\n",
        "  q[1,1] = complex(np.cos(theta),-np.sin(theta)*v[2])\n",
        "  return(q)\n",
        "\n",
        "def FromSU2(q):\n",
        "  ''' return theta, u from SU(2) representation of q'''\n",
        "  q0 = np.real(q[0,0])\n",
        "  q1 = np.real(q[0,1])\n",
        "  q2 = np.imag(q[0,1])\n",
        "  q3 = np.imag(q[0,0])\n",
        "  im = np.linalg.norm([q1,q2,q3])\n",
        "  theta = np.arctan2(im, q0)\n",
        "  u = np.array([q1,q2,q3])\n",
        "  return u, theta"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "8ZWXywBIevcT",
        "outputId": "448b185d-8a1a-461e-e019-61151b47dd59"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "v_q=[[ 6.123234e-17+0.j  2.000000e+00+0.j]\n",
            " [-2.000000e+00+0.j  6.123234e-17-0.j]]\n",
            "q=[[ 0.70710678-0.j -0.70710678-0.j]\n",
            " [ 0.70710678-0.j  0.70710678+0.j]]\n",
            "q^*=[[ 0.70710678+0.j  0.70710678+0.j]\n",
            " [-0.70710678+0.j  0.70710678-0.j]]\n",
            "v*qc=[[-1.41421356+0.j  1.41421356+0.j]\n",
            " [-1.41421356+0.j -1.41421356+0.j]]\n",
            "vr_q=[[ 2.02930727e-17+0.j  2.00000000e+00+0.j]\n",
            " [-2.00000000e+00+0.j -2.02930727e-17+0.j]]\n",
            "rotated vector is [2. 0. 0.]\n"
          ]
        }
      ],
      "source": [
        "#test by rotating y axia CCW about z axis by an angle\n",
        "v = np.array([2,0,0])\n",
        "v_q = ToSU2(v) #vector along y axis\n",
        "print(f\"v_q={v_q}\")\n",
        "rot_ang = -np.pi/2\n",
        "q_ang=rot_ang/2\n",
        "axis = np.array([1,0,0])\n",
        "q = ToSU2(axis,q_ang) #quaternion (in SU(2), 2x2 matrix rotate by 45 degrees about z axis\n",
        "qc = ToSU2(axis,-q_ang) #its conjugate\n",
        "print(f\"q={q}\")\n",
        "print(f\"q^*={qc}\")\n",
        "print(f\"v*qc={v_q @ qc}\")\n",
        "vr_q = q @ v_q @ np.conj(qc)\n",
        "print(f\"vr_q={vr_q}\")\n",
        "vr,_ = FromSU2(vr_q)\n",
        "print(f\"rotated vector is {vr}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "OyBn8Fbxq3z7"
      },
      "source": [
        "---\n",
        "**Q4**\n",
        "\n",
        "a) Write the quaternion to rotate a vector by $\\pi/4$ clockwise around an axis of $\\begin{bmatrix} 2\\\\ 2\\\\2\\end{bmatrix}$\n",
        "\n",
        "b) When you apply this to a vector $\\begin{bmatrix} 2 \\\\ 0 \\\\0\\end{bmatrix}$, what is the resultant vector?"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "bZJCnRWOrku9"
      },
      "source": [
        "Ans:\n",
        "\n",
        "(a)\n",
        "$\\theta = -\\pi / 4 \\implies \\theta / 2 = -\\pi / 8$\n",
        "\n",
        "$\\displaystyle v = \\frac{1}{\\sqrt{2^2+2^2+2^2}}\\begin{bmatrix} 2 \\\\ 2 \\\\ 2 \\end{bmatrix} = \\frac{1}{\\sqrt{3}} \\begin{bmatrix} 1 \\\\ 1 \\\\ 1 \\end{bmatrix}$\n",
        "\n",
        "$\\displaystyle \\implies q = \\cos (-\\pi/8) + \\sin(-\\pi/8) \\left( \\frac{1}{\\sqrt{3}} i + \\frac{1}{\\sqrt{3}} j + \\frac{1}{\\sqrt{3}} k \\right) = \\frac{\\sqrt{2 + \\sqrt{2}}}{2} - \\frac{\\sqrt{2 - \\sqrt{2}}}{2} \\left( \\frac{1}{\\sqrt{3}} i + \\frac{1}{\\sqrt{3}} j + \\frac{1}{\\sqrt{3}} k \\right)$\n",
        "\n",
        "$\\implies \\boxed{q = \\frac{\\sqrt{2 + \\sqrt{2}}}{2} - \\frac{\\sqrt{2 - \\sqrt{2}}}{2} \\left( \\frac{1}{\\sqrt{3}} i + \\frac{1}{\\sqrt{3}} j + \\frac{1}{\\sqrt{3}} k \\right)}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "JeZTg_3-sLuS"
      },
      "source": [
        "(b)\n",
        "$u^r = q(\\theta / 2, v) u q(\\theta / 2, v)^*$\n",
        "\n",
        "$\\displaystyle \\implies u^r = \\left[ \\frac{\\sqrt{2 + \\sqrt{2}}}{2} - \\frac{\\sqrt{2 - \\sqrt{2}}}{2\\sqrt{3}} \\left( i + j + k \\right) \\right] \\left( 2i + 0j + 0k \\right) \\left[ \\frac{\\sqrt{2 + \\sqrt{2}}}{2} + \\frac{\\sqrt{2 - \\sqrt{2}}}{2\\sqrt{3}} \\left( i + j + k \\right) \\right]$\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "LQJA0N1DsN7_",
        "outputId": "f662cdde-24a3-47ab-b491-af5c30b4fcf3"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "rotated vector is [ 1.60947571 -0.62123444  1.01175873]\n"
          ]
        }
      ],
      "source": [
        "#<Your answer here>\n",
        "\n",
        "print(f\"rotated vector is {vr}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ro5yLPtLyuOw"
      },
      "source": [
        "---\n",
        "**SE(3): Group of Rigid body Motions**\n",
        "\n",
        "3D Euclidean space can be modeled as a 3D projective space $ℙ^3$, using using 4D homogenous coordinates.\n",
        "\n",
        "Then a rigid body motion - consisting of a rotation and translation, can be represented as a linear transform using a $4 \\times 4 $ real matrix as:\n",
        "\n",
        "$T = \\begin{bmatrix} R_{3 \\times 3} & t_{3 \\times 1} \\\\0_{1 \\times 3} & 1 \\end{bmatrix} \\in SE(3)$\n",
        "\n",
        "Here the rotation is $R \\in SO(3)$ and $t$ is the translation vector.\n",
        "\n",
        "Thus any point $P = \\begin{bmatrix} p_{3\\times 1} \\\\ 1 \\end{bmatrix}$ transforms to  $P^{'}=TP = Rp + t$\n",
        "\n",
        "Consider a robot stationed in the world frame W at origin with its axes aligned with that of W.\n",
        "\n",
        "Now let it move to a new location and have a new orientation, whose coordinate frame, we call B.\n",
        "\n",
        "This motion can be described by the matrix $T_{WB} = \\begin{bmatrix}R_{WB} & t_{WB}  \\\\ 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "We can interpret it in two ways:\n",
        "\n",
        "Every point $P$ on the rigid body of the robot undergoes a motion $T_{WB}$ and hence its new coordinate $T_{WB}P$\n",
        "\n",
        "At the same time, any point $Q_B$ seen with respect to the robot's local frame B is now seen in reference frame W as\n",
        "$Q_W=T_{WB}Q_B$\n",
        "\n",
        "Imagine we have a robot arm on a mobile robot Base. Let the robot arm's end-effector move to $T_{BE}$ with respect to base and let the base move to $T_{WB}$ with resepect to world frame. Then the end-effector's position in the world frame is $T_{WE}=T_{WB}T_{BE}$\n",
        "\n",
        "\n",
        "If the end effector has a sensor and it makes a measurement for a point as $P_E$ then its location in the world frame is $P_W = T_{WE} P_E = T_{WB}T_{BE}P_E$\n",
        "\n",
        "Let <br>$T_1 = \\begin{bmatrix} R_1 & t_1 \\\\ 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "$T_2 = \\begin{bmatrix} R_2 & t_2 \\\\ 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "Then $T = T_1T_2 = \\begin{bmatrix} R_1R_2 & R_1t_2+t_1 \\\\ 0 & 1 \\end{bmatrix} \\in SE(3)$\n",
        "\n",
        "We can also check that $T^{-1} = \\begin{bmatrix} R^T & -R^Tt \\\\ 0 & 1 \\end{bmatrix} \\in SE(3)$\n",
        "\n",
        "Hence $SE(3)$ is a group.\n",
        "\n",
        "*Exponentiation & Logarithm*\n",
        "\n",
        "Analogous to SO(3), for every $T \\in SE(3)$ we can find $\\hat{U} = \\begin{bmatrix} \\omega_{\\times} & \\rho \\\\ 0 & 0 \\end{bmatrix}_{4 \\times 4}$ such that\n",
        "\n",
        "$T = e^\\hat{U}$ with $R = e ^ {\\omega_\\times}$\n",
        "\n",
        "However usually working with $\\{log(R)=\\omega_\\times, t\\}$ suffices (and we dont need to work with $log(T)=\\{\\omega_\\times, \\rho\\}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "jsaGPTiHprGq"
      },
      "source": [
        "---\n",
        "**Lie Theory for Robotics**\n",
        "\n",
        "SO(3), SE(3), SO(2), SE(2) are continuous groups - i.e. given any two elements of the group, intuitively, we can connect them with a set of intermediate elements in a \"smooth\" fashion. As an example, for SO(2), which can be visualized a circle, we can go in a smooth way between any two 2D rotations (given as two points on the circle) - by following the arc between the two. The same analogy extends to SE(3), SO(3) etc.\n",
        "\n",
        "More formally, these groups are smooth manifolds and called Lie groups after Norwegian mathematician Sophus Lie. A manifold can be imagined as a lower dimensional, continuous and differentiable set embedded in a higher dimensional space. Analogy is a cloth in 3D space. At any point on the cloth a tiny region around it looks like 2D euclidean space.\n",
        "\n",
        "So a smooth manifold for which a group operation is defined such that it forms a group,  is called a Lie group."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "KMJNRGlT56a8"
      },
      "source": [
        "---\n",
        "**Tangent space**\n",
        "\n",
        "The Lie group, G, being a manifold, is continuous and differentiable.\n",
        "\n",
        "We will also only be considering matrix lie groups - i.e. those whose elements can be represented as $n\\times n$ matrices. The value of $n$ depends on the specific Lie group.\n",
        "\n",
        "Then at any point in the group, $X \\in G$, we can imagine the existence of a tangent space (since G is a manifold and we can differentiate it), obtained by differentiating at $X$ along different directions. This tangent space at $X$ is a vector space - isomorphic to euclidean space of dimension $m \\le n^2$. The values of $m$ depends on the specific lie group. Thus locally in the tangent space, we can find $m$ basis elements, $K_1, K_2, ..., K_m$ such that any tangent vector $K=\\Sigma_i \\alpha_i K_i$.\n",
        "\n",
        "Each of these tangent vectors (including the bases), can also be represented as $n\\times n$ matrices, but not any arbitary matrices - they will have some specific constraint based on the structure of the specific lie group.\n",
        "\n",
        "We can now find a bijective correspondence between vectors of the tangent space and the elements of the lie group, via the matrix exponentiation operation as follows.\n",
        "\n",
        "Particularly, consider a small perturbation in neighbourhood of the group's identity element, $I$ along $K$, to give another element almost in the group as :\n",
        "\n",
        "$X(t,N,K) =  I + \\frac{t}{N} K$, where $t\\in ℝ$ and $K$ is a vector in the tangent space and N is any positive integer.\n",
        "\n",
        "Consider  $(I+\\frac{t}{N}K)^N$ as $N→∞$, each of X(t,N,K) will become closer to I (which belongs to G) and hence their product will become a matrix that lies on $G$.\n",
        "\n",
        "This $lim_{N→∞} (I+\\frac{t}{N}K)^N = lim_{N→∞} (I + N\\frac{t}{N}K + {N \\choose 2}\\frac{t^2}{N^2}K^2+..+ \\frac{t^N}{N^N}K^N)  = I + t K + \\frac{t^2 K^2}{2!}+\\frac{t^3 K^3}{3!}+.. = e^{tK} \\in G$\n",
        "\n",
        "Conversely, every group element can be put into a one-to-one mapping with a vector from the tangent space at Identity via the inverse  map which is the logarithm.\n",
        "\n",
        "The tangent space at identity is specified as $T_IG$ and its basis vectors are called the generators of the group.\n",
        "\n",
        "We can also find the tangent space at any other group element $X$ and that is represented as $T_XG$\n",
        "\n",
        "Analogous to above, $X(t,K) = \\lim_{N →∞} X(I+\\frac{t}{N}K) = Xe^{tK}$ where K is in the tangent space at X ($T_XG$)."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8DWD9MKsG0ws"
      },
      "source": [
        "---\n",
        "**Lie Bracket and Lie Algebra**\n",
        "\n",
        "The tangent space at identity $T_I(G)$ also written as $\\frak{g}$, is  a vector space isomorphic to some $ℝ^m$. A special product operation is defined on this under which  it is closed, thus making it an alegbra (an algebra is a vector space with a product defined).\n",
        "\n",
        "This product is called the Lie Bracket an denoted\n",
        "as  $[\\cdot,\\cdot]:\\frak{g} → \\frak{g}$\n",
        "\n",
        "This product should satisfy\n",
        "\n",
        "1. bilinearity: i.e. $[aX_1+bX_2, X_3] = a[X_1,X_3]+b[X_2,X_3]$ and similarly linear in second argument\n",
        "\n",
        "2. Alternating: i.e. $[X,X]=0$\n",
        "\n",
        "3. Jacobi identity: i.e. $[X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]] = 0$\n",
        "\n",
        "For matrix Lie algebras, this product is usually defined as:\n",
        "\n",
        "$[X,Y] = XY - YX\\ \\ \\forall X, Y \\in \\frak{g}$.\n",
        "\n",
        "Hence the tangent space at identity of the group G, with this product is also called its Lie Algebra.\n",
        "\n",
        "Note that $XY$ need not be an element of $\\frak{g}$, while $[X,Y] \\in \\frak{g}$ if $X,Y \\in \\frak{g}$.\n",
        "\n",
        "If $X,Y$ commute, then $[X,Y] = 0$\n",
        "\n",
        "Given any two elements in the Lie Algebra $X,Y \\in \\frak{g}$, then by definition $e^X, e^Y \\in G$ the corresponding Lie group. Hence by closure of group operation, $e^X \\cdot e^Y \\in G$ and  can be represented as an exponentiation of some element of the corresponding Lie algrebra as $e^X \\cdot e^Y = e^Z$ where $Z \\in \\frak{g}$.\n",
        "\n",
        "However $Z \\neq X + Y$ in general (unless X, Y commute).\n",
        "\n",
        "In general it is given by a more complex formula, the BCH formula $Z = X + Y + 1/2 [X,Y] + 1/12 [X,[X,Y]]+ .... $ (see https://en.wikipedia.org/wiki/Baker%E2%80%93Campbell%E2%80%93Hausdorff_formula)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "J8k7K0RzGv-P"
      },
      "source": [
        "---\n",
        "**Lie Algebra of rotation groups**\n",
        "\n",
        "As mentioned earlier not any $K$ will work. For the rotation groups $SO(2)$ and $SO(3)$, orthogonality and unit determinant of the corresponding matrices is needed. Hence  i.e. $det)R(\\theta)) = 1$ and $RR^T=I$. This can be only achieved if the matrices of the tangent space $K$ have a special structure.\n",
        "\n",
        "To derive this $RR^T = I$ and hence if $R=e^K$ then $R^{-1}=e^{-K} = R^T = e^{K^T}$.\n",
        "\n",
        "Therefore $K = -K^T$ i.e the matrices of lie algebra are skew-symmetric.\n",
        "\n",
        "Note also from above that $\\frac{dR}{d\\theta}|_{\\theta=0} = K$, i.e $K$ is the tangent vector at the identity element $R(0,K)=I$.\n",
        "\n",
        "For the $SO(2)$ lie group, the Lie algebra is denoted as $\\frak{so(2)}$ and for $SO(3)$ it is $\\frak{so(3)}$.\n",
        "\n",
        "\n",
        "---\n",
        "Since $SO(2)$ is the set of all rotations in 2D, it has only one free parameter, the angle of rotation $\\theta$. Hence the Lie algebra $\\frak{so(2)}$ is a vector space of $2 \\times 2$ skew symmetric matrices of dimension 1. A convenient basis vector is $z_\\times = \\begin{bmatrix} 0 & -1 \\\\ 1 & 0 \\end{bmatrix}$\n",
        "\n",
        "Thus for any $R(\\theta) \\in SO(2)$, which represents a counter clock wise (CCW) rotation about the origin, can be generated as  $R(\\theta) = e^{\\theta z_\\times} = \\begin{bmatrix} cos(\\theta) & -sin(\\theta) \\\\ sin(\\theta) & cos(\\theta) \\end{bmatrix} = cos(\\theta)I + sin(\\theta)z_\\times$\n",
        "\n",
        "Hence given an $R$, we can find the corresponding angle as:\n",
        "\n",
        "$\\theta = \\arccos (\\frac{tr(R)}{2})$\n",
        "\n",
        "---\n",
        "$SO(3)$ is the set of all rotations in 3D and has only three free parameters, the angle of rotation $\\theta,\\phi,\\gamma$ about the three axes $x,y,z$. Hence the Lie algebra $\\frak{so(3)}$ is a vector space of $3 \\times 3$ skew symmetric matrices and has dimension 3. A convenient basis vectors (and hence the generators of the Lie group SO(3)) are:\n",
        "\n",
        "$k_\\times = \\begin{bmatrix} 0 & -1 & 0 \\\\ 1 & 0 & 0 \\\\ 0 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "$j_\\times=\\begin{bmatrix} 0 & 0 & 1 \\\\ 0 & 0 & 0 \\\\ -1 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "$i_\\times=\\begin{bmatrix} 0 & 0 & 0 \\\\ 0 & 0 & -1 \\\\ 0 & 1 & 0 \\end{bmatrix}$\n",
        "\n",
        "\n",
        "Thus for any rotation by $\\theta$ CCW about an axis $\\omega = \\begin{bmatrix} \\omega_1 & \\omega_2 & \\omega_3 \\end{bmatrix}$,  $\\omega_\\times = \\omega_1 i_\\times + \\omega_2 j_\\times + \\omega_3 k_\\times = \\begin{bmatrix}0 & -\\omega_3 & \\omega_2 \\\\ \\omega_3 & 0 & -\\omega_1 \\\\ -\\omega_2 & \\omega_1 & 0 \\end{bmatrix}$\n",
        "\n",
        "\n",
        "$R = e^{\\theta \\omega_\\times}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "SsGjUU7HQ6Ad"
      },
      "source": [
        "---\n",
        "**Lie group of $SE(2)$**\n",
        "\n",
        "The lie algebra of $SE(2)$ denoted as $\\frak{se(2)}$ has three generators:\n",
        "\n",
        "$z_\\times = \\begin{bmatrix} 0 & -1 & 0 \\\\ 1 & 0 & 0 \\\\ 0 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "$tx_\\times = \\begin{bmatrix} 0 & 0 & 1 \\\\ 0 & 0 & 0 \\\\ 0 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "$ty_\\times = \\begin{bmatrix} 0 & 0 & 0 \\\\ 0 & 0 & 1 \\\\ 0 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "Note that $tx^2_\\times = ty^2_\\times = 0$\n",
        "\n",
        "An element of $\\frak{se(2)}$ can be represented as $m_\\times = \\theta z_\\times + v_1 tx_\\times + v_2 ty_\\times$. The corresponding vector is $m=\\begin{bmatrix} \\theta \\\\ v \\end {bmatrix}$ with $v=\\begin{bmatrix} v_1 \\\\ v_2 \\end{bmatrix}$. The corresponding element in $SE(2)$ is :\n",
        "\n",
        "$M = e^{m_\\times} = \\begin{bmatrix} e^{\\theta_x} & Tv \\\\ 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "Where $T = (1-\\frac{\\theta^2}{3!} + \\frac{\\theta^4}{5!}+...)\\begin{bmatrix}1 & 0 \\\\0 & 1 \\end{bmatrix} + (\\frac{\\theta}{2!} -\\frac{\\theta^3}{4!}+...)\\begin{bmatrix} 0 & -1 \\\\ 1 & 0 \\end{bmatrix}$. This can be rewritten as\n",
        "\n",
        "$T = \\frac{1}{\\theta}\\begin{bmatrix}sin(\\theta) &  -(1-cos(\\theta) \\\\ 1-cos(\\theta) & sin(\\theta) \\end{bmatrix}$\n",
        "\n",
        "Similarly $(\\theta, v)$ can be obtained from $\\begin{bmatrix} R & t \\\\ 0 & 1 \\end{bmatrix}$ by:\n",
        "\n",
        "first find $\\theta$ from $R$ and then form $T$ as above and solve for $v$ in $Tv = t$ as $v = T^{-1}t$\n",
        "\n",
        "If $A = \\frac{sin(\\theta)}{\\theta}$, $B=\\frac{1-cos(\\theta)}{\\theta}$ then $T^{-1} = \\frac{1}{A^2+B^2}\\begin{bmatrix}A & B \\\\ -B & A \\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "BJ_nN2-eRKVi"
      },
      "source": [
        "---\n",
        "**Lie group $SE(3)$**\n",
        "\n",
        "\n",
        "3D rigid body motions is described by the $4 \\times 4$ matrix:\n",
        "\n",
        "$M=\\begin{bmatrix} R & t \\\\ 0 & 1 \\end{bmatrix}$\n",
        "\n",
        "It has 6 degrees of freedom - 3 for rotation and 3 for translation.\n",
        "\n",
        "Hence the Lie algebra $\\frak{se(3)}$ has a basis set of six elements (group generators). One such set is:\n",
        "\n",
        "$k_\\times = \\begin{bmatrix} 0 & -1 & 0 &0 \\\\ 1 & 0 & 0  &0 \\\\ 0 & 0 & 0 &0 \\\\ 0 & 0 & 0 & 0\\end{bmatrix}$\n",
        "$j_\\times=\\begin{bmatrix} 0 & 0 & 1  &0 \\\\ 0 & 0 & 0 & 0 \\\\ -1 & 0 & 0 &0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}$\n",
        "$i_\\times=\\begin{bmatrix} 0 & 0 & 0 & 0 \\\\ 0 & 0 & -1 & 0 \\\\ 0 & 1 & 0 &0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "$t_x = \\begin{bmatrix} 0 & 0 & 0 & 1 \\\\ 0 & 0 & 0 & 0 \\\\ 0 & 0 & 0 & 0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}$\n",
        "$t_y = \\begin{bmatrix} 0 & 0 & 0 & 0 \\\\ 0 & 0 & 0 & 1 \\\\ 0 & 0 & 0 & 0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}$\n",
        "$t_z = \\begin{bmatrix} 0 & 0 & 0 & 0 \\\\ 0 & 0 & 0 & 0 \\\\ 0 & 0 & 0 & 1 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}$\n",
        "\n",
        "A vector $\\delta = \\begin{bmatrix} \\theta u \\\\ v \\end{bmatrix} \\in ℝ^6$, with $u, v \\in ℝ^3$ and $u^Tu=1$, has a corresponding algebra element $\\delta_\\times $ and maps to $M = e^{\\delta_\\times} \\in SE(3)$ as:\n",
        "\n",
        "$M = \\begin{bmatrix} e^{\\theta u_x} & Tv \\\\ 0 & 1 \\end{bmatrix} $\n",
        "\n",
        "with\n",
        "\n",
        "\n",
        "$R=e^{\\theta u_\\times} = I + sin(\\theta) u_\\times + (1-cos(\\theta)u^2_\\times$\n",
        "\n",
        "$T = I + \\frac{(1-cos(\\theta))}{\\theta}u_x + (1-\\frac{sin(\\theta)}{\\theta})u^2_\\times$\n",
        "\n",
        "To go from Lie group element $M$ to the tangent vector $\\delta$, we can use the formulae in the $SO(3)$ section to get $\\theta, u$ and then find $T$ and solve for $v=T^{-1}t$.\n",
        "\n",
        "A closed form of $T^{-1} = I - \\frac{\\theta}{2}u_\\times + (1-\\frac{\\theta sin(\\theta)}{2(1-cos(\\theta)})u^2_\\times$  can be used to find $v$.\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1Z0iVOZbLlkd"
      },
      "source": [
        "---\n",
        "**Q5**\n",
        "\n",
        "Find\n",
        "\n",
        "$[i_\\times,j_\\times]$\n",
        "\n",
        "$[j_\\times,k_\\times]$\n",
        "\n",
        "$[k_\\times,i_\\times]$\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nFYHuIDCLsyg"
      },
      "source": [
        "Ans:\n",
        "\n",
        "$[i_\\times,j_\\times] = i_\\times j_\\times - j_\\times i_\\times$\n",
        "\n",
        "$\n",
        "i_\\times j_\\times = \n",
        "\\begin{bmatrix} 0 & 0 & 0 & 0 \\\\ 0 & 0 & -1 & 0 \\\\ 0 & 1 & 0 &0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}\n",
        "\\begin{bmatrix} 0 & 0 & 1  &0 \\\\ 0 & 0 & 0 & 0 \\\\ -1 & 0 & 0 &0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}\n",
        "$\n",
        "\n",
        "$\n",
        "j_\\times i_\\times =\n",
        "\\begin{bmatrix} 0 & 0 & 1  &0 \\\\ 0 & 0 & 0 & 0 \\\\ -1 & 0 & 0 &0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}\n",
        "\\begin{bmatrix} 0 & 0 & 0 & 0 \\\\ 0 & 0 & -1 & 0 \\\\ 0 & 1 & 0 &0 \\\\ 0 & 0 & 0 & 0 \\end{bmatrix}\n",
        "$\n",
        "\n",
        "$\\boxed{[i_\\times,j_\\times] = k_\\times}$\n",
        "\n",
        "Similarly $\\boxed{[j_\\times,k_\\times] = i_\\times}$ and $\\boxed{[k_\\times,i_\\times] = j_\\times}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1p7cGJ13MBsA"
      },
      "source": [
        "**Q6**\n",
        "\n",
        "For any $A \\in \\frak{g}$  show that $(e^A)^{-1} = e^{-A}$\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "vgu1LCBDMIYQ"
      },
      "source": [
        "Ans:\n",
        "\n",
        "Since for any $A \\in \\frak{g}$, we have that $A$ and $-A$ commute $\\implies [A,-A] = 0$.\n",
        "\n",
        "Then, by BCH formula, $e^Ae^{-A} = e^{A + (-A)} = e^0 = I$, thereby giving that $\\boxed{(e^A)^{-1} = e^{-A}}$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "VKX8pDp0ROUl"
      },
      "source": [
        "---\n",
        "**Lie Algebra and its isomorphic Euclidean space**\n",
        "\n",
        "Since the Lie Algebra is also a vector space, it can be made isomorphic to $ℝ^m$ where $m$ is the dimension of the Lie Algebra.\n",
        "\n",
        "For example, the Lie group element $R \\in SO(3)$ can be written as an exponential of a Lie algebra element,  $R = e^{r_\\times} $ where $r_\\times \\in \\frak{so(3)}$, which itself can be written as a linear combination of the 3 basis elements (i.e. the group generators):\n",
        "\n",
        "$r_\\times = r_1 i_\\times + r_2 j_\\times + r_3 k_\\times $\n",
        "\n",
        "Hence $\\frak{so(3)}$ can be bijectively mapped to $ℝ^3$ as :\n",
        "\n",
        "$r_\\times ↔ r$ with $r = \\begin{bmatrix} r_1 \\\\ r_2 \\\\ r_3 \\end{bmatrix}$.\n",
        "\n",
        "Convenient operators hat and vee help to implement this:\n",
        "\n",
        "$\\hat{} :ℝ^3 → \\frak{so(3)}$, with $\\hat{r} = r_\\times$\n",
        "\n",
        "and\n",
        "\n",
        "$\\vee : \\frak{so(3)} → ℝ^3$ with $r_\\times^\\vee = r$\n",
        "\n",
        "Thus the three basis vectors of $ℝ^3$,\n",
        "\n",
        "$i=\\begin{bmatrix}1\\\\0\\\\0\\end{bmatrix}$\n",
        "$j=\\begin{bmatrix}0\\\\1\\\\0\\end{bmatrix}$\n",
        "$k=\\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix}$\n",
        "\n",
        "maps into $\\frak{so(3)}$ as\n",
        "\n",
        "$\\hat{i} = i_\\times$,\n",
        "$\\hat{j} = j_\\times$,\n",
        "$\\hat{k} = k_\\times$,\n",
        "\n",
        "\n",
        "We can extend the exp and log between Lie group and Lie Algebra to Lie group and the eucliean vector space as:\n",
        "\n",
        "$Exp :ℝ^3 → SO(3)$ as a composition of $\\hat{}:ℝ^3 → \\frak{so(3)}$ followed by exponentiation $e:\\frak{so(3)}$$ → SO(3)$\n",
        "So $r \\in ℝ^3$ maps to $Exp(r) = e^{\\hat{r}} \\in SO(3)$\n",
        "\n",
        "Similarly, $Log: SO(3) → ℝ^3$ does the opposite.\n",
        "\n",
        "\n",
        "Example: $Exp(\\theta i) = e^{\\hat{\\theta i}} = e^{\\theta\\hat{i}}= e^{\\theta i_\\times}$ represents a rotation of $\\theta$ Counter Clock Wise (when looking into the X axis given as vector $i$.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 55,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "5v6Z45xZ1ChD",
        "outputId": "33536453-3bef-4fa9-dceb-b345b0d5c41b"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "2D rotation in CCW by 0.7853981633974483 is \n",
            " [[ 0.70710678 -0.70710678]\n",
            " [ 0.70710678  0.70710678]]\n",
            "---\n",
            "3D rotation by 0.7853981633974483 CCW about z axis is \n",
            " [[ 0.70710678 -0.70710678  0.        ]\n",
            " [ 0.70710678  0.70710678  0.        ]\n",
            " [ 0.          0.          1.        ]]\n"
          ]
        }
      ],
      "source": [
        "#Examples:\n",
        "import scipy as sp\n",
        "#generator matrices for 2D\n",
        "zx = np.array([[0,-1],[1,0]])\n",
        "\n",
        "\n",
        "#generator matrices for 3D rotations\n",
        "kx = np.array([[0,-1,0],[1,0,0],[0,0,0]]) #z axis\n",
        "jx = np.array([[0,0,1],[0,0,0],[-1,0,0]]) #y axis\n",
        "ix = np.array([[0,0,0],[0,0,-1],[0,1,0]]) #x axis\n",
        "\n",
        "\n",
        "theta = np.pi/4\n",
        "\n",
        "#2D CCW rotation by 45\n",
        "R = sp.linalg.expm(np.pi/4*zx)\n",
        "print(f\"2D rotation in CCW by {theta} is \\n {R}\\n---\")\n",
        "\n",
        "\n",
        "#3D CCW rotation by 45 about Z axis\n",
        "R = sp.linalg.expm(np.pi/4*kx)\n",
        "print(f\"3D rotation by {theta} CCW about z axis is \\n {R}\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 56,
      "metadata": {
        "id": "KzWu-qbS39DA"
      },
      "outputs": [],
      "source": [
        "def hat(v):\n",
        "  ''' converts a vector into its  corresponding lie algebra representation which is a skew symmetric matrix'''\n",
        "  #generator matrices for 2D\n",
        "  zx = np.array([[0,-1],[1,0]])\n",
        "\n",
        "  #generator matrices for 3D\n",
        "  kx = np.array([[0,-1,0],[1,0,0],[0,0,0]]) #z axis\n",
        "  jx = np.array([[0,0,1],[0,0,0],[-1,0,0]]) #y axis\n",
        "  ix = np.array([[0,0,0],[0,0,-1],[0,1,0]]) #x axis\n",
        "\n",
        "  if len(v) == 1:\n",
        "    return v*zx\n",
        "  elif len(v) == 3:\n",
        "    return(v[0]*ix+v[1]*jx+v[2]*kx)\n",
        "  else:\n",
        "    print(f\"ERROR: len(v) is incorrect {len(v)} is not 1 or 3\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 57,
      "metadata": {
        "id": "GGn-Hbvu6Of8"
      },
      "outputs": [],
      "source": [
        "def vee(K):\n",
        "  ''' converts a skew symmetric matrix of Lie algebra to corresponding vector'''\n",
        "  if K.shape[0] == 2:\n",
        "    return K[1,0]\n",
        "  elif K.shape[0] == 3:\n",
        "    v = np.array([K[2,1],K[0,2],K[1,0]])\n",
        "    return(v)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 58,
      "metadata": {
        "id": "6qV_4Dkw1Ltp"
      },
      "outputs": [],
      "source": [
        "def Exp(v,theta):\n",
        "  '''create a rotation matrix to rotate CCW looking down into axis v'''\n",
        "  u = v/np.linalg.norm(v)\n",
        "  #using Rodrigues formula\n",
        "  return (np.eye(3) + np.sin(theta)*hat(u)+(1-np.cos(theta))*hat(u)@hat(u))\n",
        "  #direct exponentiation\n",
        "  #return (sp.linalg.expm(theta*hat(u)))\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 59,
      "metadata": {
        "id": "gvi7mK_sfipd"
      },
      "outputs": [],
      "source": [
        "def Log(R):\n",
        "  '''Return the axis, angle'''\n",
        "  theta = np.arccos((np.trace(R)-1)/2)\n",
        "  if np.sin(theta)>0.01:\n",
        "    v = vee((R-R.T)/2/np.sin(theta))\n",
        "  else: #theta too small - so use an approximation\n",
        "    v = vee((R-R.T)*(0.5+theta**2/12))\n",
        "  return v, theta"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "9xNjiKo90vjm"
      },
      "source": [
        "---\n",
        "**Q7**\n",
        "\n",
        "Repeat part b) of Q4 using exponentiation of lie theory"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "zOW7NuRD04SR"
      },
      "source": [
        "Ans: In the code below"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Ey4dYfki06Wx",
        "outputId": "7266f927-9d78-4774-fd5f-4aed8cb520b9"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "SO(3) Rotated vector is [ 1.60947571 -0.62123444  1.01175873]\n"
          ]
        }
      ],
      "source": [
        "#<Your answer here>\n",
        "\n",
        "\n",
        "print(f\"SO(3) Rotated vector is {vr}\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "JlgZ8cA4jPnn",
        "outputId": "1fa3c73b-64d4-4f91-d63e-9ba9801a134f"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "ang=0.7853981633974484 rot_ang=-0.7853981633974483\n",
            "axis=[-0.57735027 -0.57735027 -0.57735027]\n"
          ]
        }
      ],
      "source": [
        "#<Check if inverse gives the same inpus>\n",
        "axis, ang = Log(R)\n",
        "print(f\"ang={ang} rot_ang={rot_ang}\")\n",
        "print(f\"axis={axis}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "dxEpnREDDcj4"
      },
      "source": [
        "---\n",
        "**Adjoint Operator and Global and Local coordinates**\n",
        "\n",
        "The tangent space at identity, $T_I(G)$ of the Lie group $G$ can be used to \"coordinatize\" every group element. As, every group element is an exponential map of the vector in the tangent space, the vectors's coordinates in its bases (which also generators of the group), forms the coordinates of the group element.\n",
        "\n",
        "For example, for $R \\in G$, $R=e^{r_\\times}$ with $r_\\times \\in \\frak{g}$ and $r_\\times=r_1 i^1_\\times + r_2 i^2_\\times + ...+ r_n i^n_\\times$.\n",
        "\n",
        "Hence $r = (r_1, r_2, ..., r_n)$  gives the coordinates of $R$, in terms of the basis tangent vectors at the group's identity element.\n",
        "\n",
        "The tangent space at any other group element $R$, denoted as $T_R(G)$ provides a local coordinate space @ $R$.  For a vector $v$ and the corresponding lie algebra element $v_\\times$,\n",
        "\n",
        "$Y=Re^{v_\\times}$ is an element of $G$ obtained by using the tangent space @ R. The same element can also be obtained by first a composition of the lie algebra element $v_\\times^{'}$ at the identity with the group element $R$ as:\n",
        "\n",
        "$Y = Re^{v_\\times} = e^{v_\\times^{'}}R$\n",
        "\n",
        "Thus $e^{v_\\times^{'}} = Re^{v_\\times}R^{-1} = e^{Rv_\\times R^{-1}}$\n",
        "\n",
        "The last equality comes by considering the power series expansion of the exponentiation.\n",
        "\n",
        "Thus,\n",
        "\n",
        "$v_\\times^{'} = Rv_\\times R^{-1} =  Ad_R(v_\\times)$\n",
        "\n",
        "The function $Ad_R()$ is a linear operator.\n",
        "\n",
        "Hence it can be represened as a matrix operation for the corresponding vectors,\n",
        "\n",
        "$v^{'} = Ad_R v$ here $Ad_R$ is the adjoint matrix.\n",
        "\n",
        "Hence $Ad_R$ helps to map the local tangent space at R to that at the identity.\n",
        "\n",
        "\n",
        "Example: For SO(2) and SO(3)\n",
        "\n",
        "$Ad_R = R$. (as $v_\\times^{'}=Rv_\\times R^T = (Rv)_\\times$ and hence $v^{'}=Rv$. This follows from from the identity: $(Rv)_\\times = Rv_\\times R^T$ if R is a rotation matrix.)\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "2Su4l8-LTN53"
      },
      "source": [
        "---\n",
        "**Q8**\n",
        "\n",
        "Show that:\n",
        "\n",
        "a) $Ad_I$ = I\n",
        "\n",
        "b) $Ad_X Ad_Y = Ad_{XY}$\n",
        "\n",
        "c) $Ad_{X^{-1}} = Ad^{-1}_X$\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "tIm4rygjTmsY"
      },
      "source": [
        "Ans:\n",
        "\n",
        "Since $Ad_R(v_\\times) = Rv_\\times R^{-1}$, we have\n",
        "\n",
        "(a)\n",
        "$\\implies Ad_I(v_\\times) = Iv_\\times I^{-1} = I v_\\times \\implies Ad_I = I$\n",
        "\n",
        "(b)\n",
        "$\\implies Ad_X Ad_Y(v_\\times) = X(Yv_\\times Y^{-1})X^{-1} = (XY)v_\\times (XY)^{-1} = Ad_{XY}(v_\\times) \\implies Ad_X Ad_Y = Ad_{XY}$\n",
        "\n",
        "(c)\n",
        "$\\implies Ad_{X^{-1}}(v_\\times) = X^{-1}v_\\times X$\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "5BHEtlXVinu-"
      },
      "source": [
        "---\n",
        "**The Plus and Minus Operators**\n",
        "\n",
        "Let $X, Y \\in G$ and $v_x \\in \\frak{g}$ with $v \\in ℝ^m$ which is isomorphic to $\\frak{g}$\n",
        "\n",
        "Define the following two operators: $\\oplus$ & $\\ominus$\n",
        "\n",
        "$X ⊕ v = X\\cdot Exp(v)$\n",
        "\n",
        "$v ⊕ X = Exp(v)\\cdot X$\n",
        "\n",
        "$Y \\ominus X = Log(X^{-1}Y)$\n",
        "\n",
        "\n",
        "If $Y = X ⊕ v = X Exp(v)$ the $||v||=||Log(X^{-1}Y)||$ is the geodesic distance between $X,Y$, i.e. it is the distance on the manifold $G$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "VcNJb10it15N"
      },
      "source": [
        "---\n",
        "**Jacobians on Euclidean spaces**\n",
        "\n",
        "Let $f:ℝ^n → ℝ^m$ be a vector valued, continuous, differentiable function with $f(x)=\\begin{bmatrix}f_1(x)\\\\f_2(x)\\\\...\\\\f_m(x)\\end{bmatrix}$ and $x \\in ℝ^n$\n",
        "\n",
        "\n",
        "We can write each $f_i(x+h) \\approx f_i(x) + \\nabla f_i h + h^TH_ih + $ higher order terms in $h$. Here $\\nabla f_i = \\begin{bmatrix} \\frac{\\delta f_i}{δx_1} \\\\ \\frac{\\delta f_i}{δx_2} \\\\ ... \\\\ \\frac{\\delta f_i}{δx_n}  \\end{bmatrix} $ is the gradient.\n",
        "\n",
        "Hence for the vector function, a first order approximation is:\n",
        "\n",
        "$f(x+h) \\approx f(x) + Jh$ where $J = \\begin{bmatrix} \\nabla^Tf_1 \\\\ \\nabla^T f_2 \\\\ ... \\\\ \\nabla^T f_m \\end{bmatrix}$ is the Jacobian of $f$ and is a $m \\times n$ matrix that transforms differentials in the domain $h=\\Delta x = \\begin{bmatrix} \\Delta x_1 \\\\ \\Delta x_2 \\\\ ... \\\\ \\Delta x_n \\end{bmatrix}$ into differentials in the range as $\\Delta f = J \\Delta x$.\n",
        "\n",
        "\n",
        "Note that the columns of $J$ are $j_i = \\begin{bmatrix} \\frac{\\delta f_1}{\\delta x_i} \\\\ \\frac{\\delta f_2}{\\delta x_i} \\\\ ... \\\\ \\frac{\\delta f_m}{\\delta x_i} \\end{bmatrix}$, i.e. partial derivatives along the $i^{th}$ unit basis vector of the domain.\n",
        "\n",
        "\n",
        "\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "bethNTpHxkwi"
      },
      "source": [
        "---\n",
        "**Jacobians of functions on Lie groups**\n",
        "\n",
        "Let $f:G(n) → H(m)$ be a scalar valued continuous and differentiable function from an n-dimensional manifold to an m-dimensional manifold.\n",
        "\n",
        "Taking the analogy with function's on Euclidean spaces, the $i^{th}$ column of the Jacobian matrix is the  partial derivative along the $i^{th}$ basis vector of the tangent space of G (which is $\\frak{g}$).\n",
        "\n",
        "Let $1_\\times, 2_\\times, ..., n_\\times$ be the basis tangent vectors in $\\frak{g}$.\n",
        "\n",
        "Then $X \\in G$ has the coordinates $x=(x_1, x_2, .., x_n)$ where\n",
        "\n",
        "$X = e^{x_1 1_\\times + x_2 2_\\times + ... + x_n n_\\times}$.\n",
        "\n",
        "Then the $i^{th}$ column of Jacobian $J = \\begin{bmatrix} j_1 & j_2 & ... & j_n \\end{bmatrix}$ where\n",
        "\n",
        "$j_{i} = limit_{h\\rightarrow 0} \\frac{f (X ⊕ h \\bf{i}) ⊖ f(X)}{h}$ is a m dimensional column vector in the tangent space at $f(X) \\in H(m)$, where $\\bf{i}$ is the $i^{th}$ direction in the vector space isomorphic to the tangent space at $X \\in G(n)$.\n",
        "\n",
        "$J$ is a $m \\times n$ matrix that maps tangent space at $X$ to tangent space at $f(X)$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "KTnT98jnCvjN"
      },
      "source": [
        "---\n",
        "\n",
        "*Example 1:*\n",
        "\n",
        "Let $f(X) = X$ (we will expect Identity jacobian),\n",
        "\n",
        "then $j_{i} = limit_{h → 0} \\frac{(X \\oplus h\\bf{i}) \\ominus X }{h}$\n",
        "\n",
        "$(X \\oplus h\\bf{i})$$ \\ominus X = Log(X^{-1}Xe^{hi_\\times}) = h\\bf{i}$\n",
        "\n",
        "Therefore $j_i = limit_{h→0} \\frac{h\\bf{i}}{h} = \\bf{i}$ which is a column vector of all zeros except a 1 in the $i^{th}$ row.\n",
        "\n",
        "Hence $J=I$ the identity matrix.\n",
        "\n",
        "*Example 2:*\n",
        "\n",
        "Let $f(X) = X^{-1}$,\n",
        "\n",
        "then $j_{i} = limit_{h → 0} \\frac{(X \\oplus h\\bf{i})^{-1} \\ominus X^{-1} }{h}$\n",
        "\n",
        "$(X \\oplus h\\bf{i})^{-1} $$\\ominus X^{-1} = Log((X^{-1})^{-1}e^{-hi_\\times}X^{-1}) = Log(Xe^{-hi_\\times}X^{-1}) = -hAd_X\\bf{i}$\n",
        "\n",
        "Therefore $j_i = limit_{h→0} \\frac{-hAd_X\\bf{i}}{h} = -Ad_X\\bf{i}$.\n",
        "\n",
        "Hence $J=-Ad_X$ .\n",
        "\n",
        "*Example 3:*\n",
        "\n",
        "Let $f:SO(3) \\times ℝ^3 → ℝ^3$ where $f(R,p) = Rp$\n",
        "\n",
        "Then $J_p = R$, the jacobian with respect to $p$.\n",
        "\n",
        "The jacobian with respect to R is $J_R = \\begin{bmatrix} j_1 & j_2 & j_3\\end{bmatrix}$.\n",
        "\n",
        "$j_i = lim_{h→0} \\frac{(R⊕h\\bf{i})p⊖Rp}{h}$\n",
        "\n",
        "$(R⊕h\\bf{i})$$p⊖Rp = Re^{hi_\\times}p - Rp \\approx R(I+hi_\\times)p-Rp = Ri_\\times ph = -Rp_\\times \\bf{i}$$h$ . The approximation is for small $h$.\n",
        "\n",
        "hence $j_i$ is the $i^{th}$ column of $-Rp_\\times$\n",
        "\n",
        "and $J = -Rp_\\times$\n",
        "\n",
        "(The above comes about because the cross product is anti-commutative, and $a \\times b = a_\\times b = -b \\times a = =b_\\times a$\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ZbTkzA_bMVHD"
      },
      "source": [
        "---\n",
        "**Q9**\n",
        "\n",
        "Using the calculus explained above, show that\n",
        "\n",
        "$tr(RA)$ has an extrema at $R=I$, where $R$ a rotation matrix and $A$ a positive definite matrix"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "YTCrP7-xOqE8"
      },
      "source": [
        "Ans:\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 60,
      "metadata": {
        "id": "y_2S5d6BbrXf"
      },
      "outputs": [],
      "source": [
        "def ExpSE3(theta, u, v):\n",
        "  '''Convert the tangent vector [theta*u, v] to a SE(3) matrix. u is normalized to 1'''\n",
        "\n",
        "  u = u/np.linalg.norm(u) #normalized just in case it isnt\n",
        "  #using Rodrigues formula\n",
        "  u_x = hat(u)\n",
        "  R = np.eye(3) + np.sin(theta)*u_x+(1-np.cos(theta))*u_x@u_x\n",
        "  if (theta > 0.01):\n",
        "    T = np.eye(3) + (1-np.cos(theta))/theta*u_x + (1-np.sin(theta)/theta)*u_x@u_x\n",
        "  else: #theta too small, use Taylor expansion\n",
        "    T = np.eye(3) + theta/2*u_x+theta**2/6*u_x@u_x\n",
        "  M = np.zeros((4,4))\n",
        "  M[:3,:3] = R\n",
        "  M[:3,3] = T@v\n",
        "  M[3,3] = 1\n",
        "  return(M)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 61,
      "metadata": {
        "id": "PUCOGYE8eEyy"
      },
      "outputs": [],
      "source": [
        "def LogSE3(M):\n",
        "  '''return the tangent vector: theta, u, v with u being unit length'''\n",
        "  R = M[:3,:3]\n",
        "  theta = np.arccos((np.trace(R)-1)/2)\n",
        "  if np.sin(theta)>0.01:\n",
        "    u = vee((R-R.T)/2/np.sin(theta))\n",
        "  else: #theta too small - so use an approximation\n",
        "    u = vee((R-R.T)*(0.5+theta**2/12))\n",
        "\n",
        "  u_x = hat(u)\n",
        "  T_inv = np.eye(3) - theta/2*u_x + (1-theta*np.sin(theta)/(2*(1-np.cos(theta))))*u_x @ u_x\n",
        "\n",
        "  v = T_inv @ M[:3,3]\n",
        "\n",
        "  return theta, u , v\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 62,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "lxjUXPNNelAD",
        "outputId": "82ce03b2-7677-4776-bc1b-122f4f43b050"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "SE(3) transformed vector is [1.6074231  1.08003001 0.         1.        ]\n"
          ]
        }
      ],
      "source": [
        "#Test the above functions\n",
        "u = np.array([0,0,1])\n",
        "u = u/np.linalg.norm(u)\n",
        "v = np.array([1,0,0])\n",
        "#theta = 0\n",
        "theta = np.pi/4\n",
        "M = ExpSE3(theta, u, v)\n",
        "x = np.array([1,0,0,1])\n",
        "xr = M @ x\n",
        "\n",
        "print(f\"SE(3) transformed vector is {xr}\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 63,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 485
        },
        "id": "ANVdohN2ry5U",
        "outputId": "fc6d3f8f-43a4-4e22-9ce0-74f0b8744e8a"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "1.5707963267948966 [0. 0. 1.] [19.63495408 -3.92699082  1.        ]\n",
            "Error in reconstructed Goal Pose 1.7763568394002505e-15\n"
          ]
        },
        {
          "data": {
            "text/plain": [
              "<matplotlib.image.AxesImage at 0x1413a2880>"
            ]
          },
          "execution_count": 63,
          "metadata": {},
          "output_type": "execute_result"
        },
        {
          "data": {
            "image/png": "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",
            "text/plain": [
              "<Figure size 640x480 with 1 Axes>"
            ]
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "source": [
        "# Example, goal pose\n",
        "\n",
        "#rotate by 90 CCW about Z axis\n",
        "goal_pose_R = Exp(np.array([0,0,1.]),np.pi/2)\n",
        "#translate to 15,10,0\n",
        "goal_pose_t = np.array([15,10.,1])\n",
        "\n",
        "#make the goal_pose SE(3) matrix\n",
        "goal_pose = np.zeros((4,4))\n",
        "goal_pose[:3,:3] = goal_pose_R\n",
        "goal_pose[:3,3] = goal_pose_t\n",
        "goal_pose[3,3] = 1\n",
        "\n",
        "#Find its tangent vector\n",
        "theta, u, v = LogSE3(goal_pose)\n",
        "print(theta, u, v)\n",
        "\n",
        "#reconstruct SE(3) via exponentiation\n",
        "M = ExpSE3(theta,u,v)\n",
        "print(f\"Error in reconstructed Goal Pose {np.linalg.norm(M-goal_pose)}\")\n",
        "\n",
        "#visualze the pose\n",
        "img = np.zeros((100,100,3),dtype=np.uint8)\n",
        "\n",
        "#camera intrinsic matrix - with focal length 1, center of image at (50,50)\n",
        "C = np.array([[1.0,0,50],[0,1,50],[0,0,1.]])\n",
        "\n",
        "rvec = M[:3,:3]\n",
        "tvec = M[:3,3]\n",
        "\n",
        "#Axis length is 4.\n",
        "img=cv2.drawFrameAxes(img,C,distCoeffs=0,rvec=theta*u,tvec=tvec,length=4.0,thickness=1)\n",
        "plt.imshow(img)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "uaPGcIdqxnE6"
      },
      "source": [
        "---\n",
        "The axes colors in the above picture are: Blue (X), Green (Y) and Red (Z). In the image plane, X axis is along  top left-to-right, Y axis is left top-to-bottom and Z axis is into the screen.\n",
        "\n",
        "The origin is (O) and the exis endpoints are X, Y, Z 4 units away from it (these are not labeled.\n",
        "\n",
        "The focal length of the camera is 1.\n",
        "\n",
        "They have a length of 4 units in 3D world, but when projected to 2D for a camera, the axis OZ appears much longer - this is because of parallax. Point Z apparently moves a smaller amount than O, X, Y as it is farther away from the camera."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "jXlo8xCOzm0s"
      },
      "source": [
        "---\n",
        "**Q10**\n",
        "\n",
        "Interpolate using four steps, to reach the goal pose,\n",
        "from the starting pose of $\\begin{bmatrix} I & 0 \\\\ 0 & 1 \\end{bmatrix}$ and plot it."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 832
        },
        "id": "LbZUUIJ1vung",
        "outputId": "f4abd898-a239-4733-f9fe-a97fa2d8a483"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "[[ 1.11022302e-16 -1.00000000e+00  0.00000000e+00  1.50000000e+01]\n",
            " [ 1.00000000e+00  1.11022302e-16  0.00000000e+00  1.00000000e+01]\n",
            " [ 0.00000000e+00  0.00000000e+00  1.00000000e+00  1.00000000e+00]\n",
            " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
            "0.39269908169872414\n",
            "[ 4.90873852 -0.9817477   0.25      ]\n",
            "[0. 0. 1.]\n",
            "[[ 0.92387953 -0.38268343  0.          4.97384407]\n",
            " [ 0.38268343  0.92387953  0.         -0.00520274]\n",
            " [ 0.          0.          1.          0.25      ]\n",
            " [ 0.          0.          0.          1.        ]]\n",
            "[[ 0.70710678 -0.70710678  0.          9.57106781]\n",
            " [ 0.70710678  0.70710678  0.          1.89339828]\n",
            " [ 0.          0.          1.          0.5       ]\n",
            " [ 0.          0.          0.          1.        ]]\n",
            "[[ 0.38268343 -0.92387953  0.         13.09178558]\n",
            " [ 0.92387953  0.38268343  0.          5.40675826]\n",
            " [ 0.          0.          1.          0.75      ]\n",
            " [ 0.          0.          0.          1.        ]]\n",
            "[[-8.93873052e-17 -1.00000000e+00  0.00000000e+00  1.50000000e+01]\n",
            " [ 1.00000000e+00 -1.24206230e-16  0.00000000e+00  1.00000000e+01]\n",
            " [ 0.00000000e+00  0.00000000e+00  1.00000000e+00  1.00000000e+00]\n",
            " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n"
          ]
        },
        {
          "data": {
            "image/png": "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",
            "text/plain": [
              "<Figure size 640x480 with 1 Axes>"
            ]
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "source": [
        "#<your code>\n"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "include_colab_link": true,
      "provenance": []
    },
    "kernelspec": {
      "display_name": "base",
      "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.9.19"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}