doc/slam_se2_derivation.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "slam_se2_derivation.ipynb",
      "provenance": [],
      "collapsed_sections": []
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "B3zrmbCktgag"
      },
      "source": [
        "The pose can be represented as a homogeneous transformation matrix:\n",
        "\n",
        "$X=\\begin{bmatrix} R & t\\\\ 0 & 1 \\end{bmatrix} = \\begin{bmatrix} cos(\\theta) & -sin(\\theta) & x\\\\ sin(\\theta) & cos(\\theta) & y \\\\ 0 & 0 & 1 \\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "h4m08C0O7i-h"
      },
      "source": [
        "Or as a 3 compoent vector:\n",
        "\n",
        "$x=[x, y, \\theta]$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1RNq79jS757U"
      },
      "source": [
        "We can define the functions to convert pose vector to the transformation matrix and vice versa:"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "28l083Aj8Jl_"
      },
      "source": [
        "import numpy as np\n",
        "from numpy.linalg import inv\n",
        "\n",
        "def t2v(tr):\n",
        "    # homogeneous transformation to vector\n",
        "    v = np.zeros((3,1))\n",
        "    v[:2, 0] = tr[:2,2]\n",
        "    v[2] = np.arctan2(tr[1,0], tr[0,0])\n",
        "    return v\n",
        "\n",
        "def v2t(v):\n",
        "    # vector to homogeneous transformation\n",
        "    c = np.cos(v[2])\n",
        "    s = np.sin(v[2])\n",
        "    tr = np.array([[c, -s, v[0]],\n",
        "                   [s,  c, v[1]],\n",
        "                   [0,  0,  1]])\n",
        "    return tr"
      ],
      "execution_count": 2,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "d0VVlVSqCfZ7"
      },
      "source": [
        "The pose graph edge error function can be written as the following form:\n",
        "\n",
        "$e_{ij}(x_i,x_j)=t2v(Z_{ij}^{-1}(X_{i}^{-1}X_{j}))$\n",
        "\n",
        "Where $Z_{ij} = \\begin{bmatrix} R'& t'\\\\ 0 & 1 \\end{bmatrix}$ is the measurement associated with pose graph edge. \n",
        "\n",
        "Notice that the position and orientation difference between the pose $x_i$  and the pose $x_j$ is written in a form of the transformation matrix. We can show that the multiplication of the two transformation matrices can express their difference.  \n",
        "\n",
        "Let $X_{wi}$ represents the pose $i$ in the world coordinate system $w$, and also represents the conversion matrix that converts a point in the coordinate system $i$ to the world coordinate system $w$. Similarly $X_{wj}$ is the pose $j$ in the world coordinate system $w$, and represents the conversion matrix that converts a point in the coordinate system $j$ to the world coordinate system $w$.\n",
        "\n",
        "So the following umtiplication $X_{wi}^{-1}\\cdot X_{wj} = X_{iw}\\cdot X_{wj}=X_{ij}$ represents the conversion matrix from the coordinate system $j$ to the coordinate system $i$.\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 68
        },
        "id": "yDa5DK4AVHw6",
        "outputId": "ae0b1b62-6845-482c-dd4a-f09e13eb4dfd"
      },
      "source": [
        "# Pose i\n",
        "v_i = np.array([0, 0, 0])\n",
        "\n",
        "# Pose j\n",
        "v_j = np.array([0.7, 0, np.pi / 2])\n",
        "\n",
        "# Edge measurement\n",
        "v_z = np.array([1, 0, np.pi / 3])\n",
        "\n",
        "def calculate_error(x_i, x_j, z):\n",
        "  # Create transformation matrices from vectors\n",
        "  t_i = v2t(x_i)\n",
        "  t_j = v2t(x_j)\n",
        "  t_z = v2t(z)\n",
        "\n",
        "  # Get rotation matrices\n",
        "  r_i = t_i[:2,:2]\n",
        "  r_z = t_z[:2,:2]\n",
        "\n",
        "  # Calculate error vector\n",
        "  e = t2v(inv(t_z) @ (inv(t_i) @ t_j))\n",
        "  return e\n",
        "\n",
        "e = calculate_error(v_i, v_j, v_z)\n",
        "display(e)"
      ],
      "execution_count": 196,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "array([[-0.15      ],\n",
              "       [ 0.25980762],\n",
              "       [ 0.52359878]])"
            ]
          },
          "metadata": {}
        }
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 68
        },
        "id": "M79Ho5K3_kjc",
        "outputId": "6ec6d80b-2b40-400c-87f1-6fc86c81a5a2"
      },
      "source": [
        "# Pose i\n",
        "v_i = np.array([0, 0, 0])\n",
        "\n",
        "# Pose j\n",
        "v_j = np.array([1, 0, np.pi / 3])\n",
        "\n",
        "# Edge measurement\n",
        "v_z = np.array([1, 0, np.pi / 3])\n",
        "\n",
        "e = calculate_error(v_i, v_j, v_z)\n",
        "display(e)"
      ],
      "execution_count": 195,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "array([[0.00000000e+00],\n",
              "       [0.00000000e+00],\n",
              "       [4.06369831e-17]])"
            ]
          },
          "metadata": {}
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "CtgEwKvM0RTz"
      },
      "source": [
        "Error should take a value of zero if:\n",
        "\n",
        "$Z_{ij} = (X_{i}^{-1}X_{j})$"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 68
        },
        "id": "gflX3ZL_0g-Z",
        "outputId": "ca2797a8-6e0b-434b-933e-34ff0bb87a52"
      },
      "source": [
        "# Pose i\n",
        "v_i = np.array([0, 0, 0])\n",
        "\n",
        "# Pose j\n",
        "v_j = np.array([1, 0, np.pi / 3])\n",
        "\n",
        "# Edge measurement\n",
        "v_z = np.array([1, 0, np.pi / 3])\n",
        "\n",
        "e = calculate_error(v_i, v_j, v_z)\n",
        "display(e)"
      ],
      "execution_count": 194,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "array([[0.00000000e+00],\n",
              "       [0.00000000e+00],\n",
              "       [4.06369831e-17]])"
            ]
          },
          "metadata": {}
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "HFYD7o-A4GOx"
      },
      "source": [
        "The next important part is calculating Jacobian of the error function. Lets make some preparations."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "VbKdXLFLNS8M"
      },
      "source": [
        "According to the previous explanation we have:\n",
        "\n",
        "$X_{i}^{-1}X_{j} = \\begin{bmatrix} R & t\\\\ 0 & 1 \\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "e9pQPaRZpk69"
      },
      "source": [
        "Inversion of a block transformation matrix can be expresed with the following formula:\n",
        "\n",
        "$\\begin{bmatrix} R & t\\\\ 0 & 1 \\end{bmatrix}^{-1} = \\begin{bmatrix} \\begin{bmatrix} I & t\\\\ 0 & 1 \\end{bmatrix} & \\begin{bmatrix} R & 0 \\\\ 0 & 1 \\end{bmatrix} \\end{bmatrix}^{-1} = \\begin{bmatrix} \\begin{bmatrix} R & 0 \\\\ 0 & 1 \\end{bmatrix}^ {-1} & \\begin{bmatrix} I & t\\\\ 0 & 1 \\end{bmatrix}^{-1} \\end{bmatrix}=\\begin{bmatrix} \\begin{bmatrix} R^{T} & 0\\\\ 0 & 1 \\end{bmatrix}&\\begin{bmatrix} I & -t\\\\ 0 & 1 \\end{bmatrix} \\end{bmatrix}= \\begin{bmatrix} R^{T} & -R^{T}t\\\\ 0 & 1 \\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "n6KmS9VqoDqc"
      },
      "source": [
        "So we can rewrite the error function in the following form:\n",
        "\n",
        "$Z_{ij}^{-1}(X_{i}^{-1}X_{j}) = \\begin{bmatrix} R' & t'\\\\ 0 & 1 \\end{bmatrix}^{-1} \\begin{bmatrix} R & t\\\\ 0 & 1 \\end{bmatrix}=\\begin{bmatrix} R'^{T} & -R't'\\\\ 0 & 1 \\end{bmatrix}\\begin{bmatrix} R & t\\\\ 0 & 1 \\end{bmatrix}=\\begin{bmatrix} R'^{T}R & Rt-R't'\\\\ 0 & 1 \\end{bmatrix}=\\begin{bmatrix}R'^{T}R & R'^{T}(t-t')\\\\0 & 1\\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "25iy8moPPQ2o"
      },
      "source": [
        "And the same function in the vectorized form:\n",
        "\n",
        "$e_{ij}(x_i,x_j)=Z_{ij}^{-1}(\\begin{bmatrix}R_i^{T}(t_j-t_i)\\\\ \\theta_j - \\theta_i \\end{bmatrix})$\n",
        "\n",
        "$\\Delta t_{ij} = R_{z}^{T}\\begin{bmatrix} R_{i}^{T}(\\begin{bmatrix} x_j \\\\ y_j\\end{bmatrix} - \\begin{bmatrix} x_i \\\\ y_i\\end{bmatrix}) - \\begin{bmatrix} x_z \\\\ y_z\\end{bmatrix})\\end{bmatrix}$\n",
        "\n",
        "$\\Delta \\theta_{ij} = (\\theta_j - \\theta_i) - \\theta_z$\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8ahGp3kR8mfC"
      },
      "source": [
        "The form of the $X$ vector for the optimization problem - is one long vector stacked from all pose vectors:\n",
        "\n",
        "$x =[(x_1, y_1, \\theta_1), (x_2, y_2, \\theta_2), ...., (x_n, y_n, \\theta_n)]$\n",
        "\n",
        "Our error term doesn't depend on all state variables but on only on 3 values blocks related to the $i$-th and $j$-th poses.\n",
        "\n",
        "And this fact is also visible in the form of Jacobian:\n",
        "\n",
        "$J_{ij} = \\begin{bmatrix} 0 & ... & 0 & A_{ij} &  0 & ... & 0 & B_{ij} & 0 & ... & 0 \\end{bmatrix}$\n",
        "\n",
        "$A_{ij} = \\frac{\\partial e_{ij}(x_i)}{\\partial x_i}$\n",
        "\n",
        "$B_{ij} = \\frac{\\partial e_{ij}(x_j)}{\\partial x_j}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "AHnrHaqyA3ZG"
      },
      "source": [
        "So for $A_{ij}$ and $B_{ij}$ we have the following formulas:\n",
        "\n",
        "$A_{ij} = \\begin{bmatrix} -R_{z}^T R_{i}^T &  R_{z}^T \\frac{\\partial R_{i}^T}{\\partial \\theta_i} (t_j - t_i) \\\\ 0 & -1 \\end{bmatrix}$\n",
        "\n",
        "where:\n",
        "\n",
        "1. the 3x2 block $\\begin{bmatrix}-R_{z}^T R_{i}^T \\\\ 0 \\end{bmatrix}$ is the partial derrivative of $\\Delta t_{ij}$ in the top and  $\\Delta \\theta_{ij}$ in the bottom w.r.t $\\left[x_i y_i\\right]$\n",
        "\n",
        "2. the 3x1 block $\\begin{bmatrix} R_{z}^T \\frac{\\partial R_{i}^T}{\\partial \\theta_i} (t_j - t_i) \\\\ -1 \\end{bmatrix}$ is the partial derrivative of $\\Delta t_{ij}$ in the top and  $\\Delta \\theta_{ij}$ in the bottom w.r.t $\\theta_i$\n",
        "\n",
        "3. $\\frac{\\partial R_{i}^T}{\\partial \\theta_i} = \\begin{bmatrix} -sin(\\theta_i) & cos(\\theta_i) \\\\  -cos(\\theta_i) & -sin(\\theta_i) \\end{bmatrix}$\n",
        "\n",
        "In the same manner we take partial derrivatives w.r.t $\\left[x_j y_j\\right]$ and $\\theta_j$:\n",
        "\n",
        "\n",
        "$B_{ij} = \\begin{bmatrix} R_{z}^T R_{i}^T &  0 \\\\ 0 & 1 \\end{bmatrix}$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "yDQY_ZaZEY7-"
      },
      "source": [
        "And the following code shows the corresponding calculations in Python:"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "0sNW_96rEQKf",
        "outputId": "c0631417-5fdf-425a-a997-c5f6448847e7"
      },
      "source": [
        "si = np.sin(v_i[2])\n",
        "ci = np.cos(v_i[2])\n",
        "dr_i = np.array([[-si, ci], [-ci, -si]]).T\n",
        "dt_ij = np.array([v_j[:2] - v_i[:2]]).T\n",
        "\n",
        "t_i = v2t(v_i)\n",
        "t_j = v2t(v_j)\n",
        "r_i = t_i[:2,:2]\n",
        "r_z = t_z[:2,:2]\n",
        "\n",
        "a_ij = np.vstack((np.hstack((-r_z.T @ r_i.T, (r_z.T @ dr_i.T) @ dt_ij)), \n",
        "                         [0, 0, -1]))\n",
        "print(f'The shape of A_ij is {a_ij.shape}')\n",
        "\n",
        "b_ij = np.vstack((np.hstack((r_z.T @ r_i.T, np.zeros((2,1)))),\n",
        "                         [0, 0, 1]))\n",
        "print(f'The shape of B_ij is {b_ij.shape}')"
      ],
      "execution_count": 192,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "The shape of A_ij is (3, 3)\n",
            "The shape of B_ij is (3, 3)\n"
          ]
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "WJPZhrhC-CBQ"
      },
      "source": [
        "Having jacobians we can calulate blocks of the H and b matrices:"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "X9s7u5sq8i9T"
      },
      "source": [
        "omega = np.identity(3) # it should be the information matrix associated with the edge\n",
        "h_ii =  a_ij.T @ omega @ a_ij\n",
        "h_ij =  a_ij.T @ omega @ b_ij\n",
        "h_jj =  b_ij.T @ omega @ b_ij\n",
        "b_i  = -a_ij.T @ omega @ e\n",
        "b_j  = -b_ij.T @ omega @ e"
      ],
      "execution_count": 197,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "4SghWy7I_1I4"
      },
      "source": [
        "And because we calculated only partials blocks for global H and b matrices we have to insert them into apropriate places:"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "WBY5lQSOADJP"
      },
      "source": [
        "# Create new H and b matrices in the begining of an optimization iteration\n",
        "\n",
        "num_nodes = 6  # should be taken from a graph\n",
        "num_params = 3  # tx, ty, theta\n",
        "H = np.zeros((num_nodes * num_params, num_nodes * num_params)) \n",
        "b = np.zeros((num_nodes * num_params, 1)) \n",
        "\n",
        "def id2index(id):\n",
        "  return slice((num_params*id), (num_params*(id+1)))\n",
        "\n",
        "i_id = 0\n",
        "j_id = 2\n",
        "H[id2index(i_id), id2index(i_id)] += h_ii\n",
        "H[id2index(i_id), id2index(j_id)] += h_ij\n",
        "H[id2index(j_id), id2index(i_id)] += h_ij.T\n",
        "H[id2index(j_id), id2index(j_id)] += h_jj\n",
        "b[id2index(i_id)] += b_i\n",
        "b[id2index(j_id)] += b_j"
      ],
      "execution_count": 212,
      "outputs": []
    }
  ]
}