{
  "nbformat": 4,
  "nbformat_minor": 2,
  "metadata": {
    "colab": {
      "name": "SO3_SE3_spatialmath.ipynb",
      "provenance": [],
      "collapsed_sections": [],
      "toc_visible": true,
      "authorship_tag": "ABX9TyMtkdYRrFRWph2TDJepJqtb",
      "include_colab_link": true
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "source": [
        "# SO3_SE3_spatialmath\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/LinkHS/Colab/blob/main/IMU/SO3_SE3_spatialmath.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ],
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "source": [
        "# !pip install spatialmath-python"
      ],
      "outputs": [],
      "metadata": {
        "id": "LyBaaF11DMtU"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "## spatialmath"
      ],
      "metadata": {
        "id": "DDfp-SXPteva"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "source": [
        "from spatialmath import SO3"
      ],
      "outputs": [],
      "metadata": {
        "id": "Q0oUM0ufHdf0"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "A rotation of 0.3 radians about the x-axis:"
      ],
      "metadata": {
        "id": "XJVmBCqaIO2P"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "source": [
        "R1 = SO3.Rx(0.3)\n",
        "R1"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "  \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0       \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0.9553  \u001b[0m \u001b[38;5;1m-0.2955  \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0.2955  \u001b[0m \u001b[38;5;1m 0.9553  \u001b[0m  \u001b[0m\n"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 3
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "gafYeRVZHTuY",
        "outputId": "1fffd8e0-683c-4cbf-9be1-a3806f622834"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "A rotation of 30 deg about the z-axis:"
      ],
      "metadata": {
        "id": "qyIhRmijIdvr"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "source": [
        "R2 = SO3.Rz(30, 'deg')\n",
        "R2"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "  \u001b[38;5;1m 0.866   \u001b[0m \u001b[38;5;1m-0.5     \u001b[0m \u001b[38;5;1m 0       \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0.5     \u001b[0m \u001b[38;5;1m 0.866   \u001b[0m \u001b[38;5;1m 0       \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 1       \u001b[0m  \u001b[0m\n"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 4
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "_9ZF4p8WHfbu",
        "outputId": "7a6e8f12-a4da-41c4-b7ba-ce6b1072247e"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "source": [
        "R2.angvec()"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "(0.5235987755982988, array([0., 0., 1.]))"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 5
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "o7E4a4q4Ne6l",
        "outputId": "aa09ea1a-5ad4-49d5-ce57-fbadec86013b"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "## 李群李代数\n",
        "![](https://images-picgo.oss-cn-shanghai.aliyuncs.com/images/Colab/20210812165059.png)\n"
      ],
      "metadata": {
        "id": "RHp4m9jEKZJo"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "### 特殊正交群 $SO(3)$\n",
        "三维旋转矩阵$\\boldsymbol{R}$构成了特殊正交群$SO(3)$：\n",
        "\n",
        "$$\n",
        "SO(3)=\\left\\{\\boldsymbol{R} \\in \\mathbb{R}^{3 \\times 3} \\mid \\boldsymbol{R} \\boldsymbol{R}^{T}=\\boldsymbol{I}, \\operatorname{det}(\\boldsymbol{R})=1\\right\\}\n",
        "$$\n",
        "\n",
        "三维旋转向量$\\phi$是一种李代数$so(3)$：\n",
        "\n",
        "$$\n",
        "so(3) = \\left\\{\\phi \\in \\mathbb{R}^{3}, \\Phi = \\boldsymbol{\\phi}^{\\wedge} \\in \\mathbb{R}^{3 \\times 3}\\right\\}\n",
        "$$\n",
        "\n",
        "其对应的反对称矩阵如下：\n",
        "\n",
        "$$\n",
        "\\boldsymbol{\\phi}^{\\wedge} = \\boldsymbol{A} =\n",
        "\\left[\\begin{array}{ccc}\n",
        "0 & -\\phi_{3} & \\phi_{2} \\\\\n",
        "\\phi_{3} & 0 & -\\phi_{1} \\\\\n",
        "-\\phi_{2} & \\phi_{1} & 0\n",
        "\\end{array}\\right],\\quad \\boldsymbol{A}^{\\vee} = \\phi\n",
        "$$\n",
        "\n",
        "\n",
        "两者转换公式如下：\n",
        "\n",
        "$$\n",
        "\\boldsymbol{R}=\\exp \\left(\\boldsymbol{\\phi}^{\\wedge}\\right)\n",
        "$$\n",
        "\n",
        "$$\n",
        "\\boldsymbol{R}^{-1}=\\exp \\left((-\\boldsymbol{\\phi})^{\\wedge}\\right)\n",
        "$$\n",
        "\n",
        "$$\n",
        "\\phi = \\ln \\left(\\exp \\left(\\boldsymbol{\\phi}^{\\wedge}\\right)\\right)^{\\vee} = \\ln(\\boldsymbol{R})^{\\vee}\n",
        "$$\n",
        "\n",
        "我们使用了$^\\wedge$和$^\\vee$符号来指代“从向量到矩阵”和“从矩阵到向量”的关系。"
      ],
      "metadata": {
        "id": "Yy8Jj4Ty3yql"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "三维向量的反对称矩阵："
      ],
      "metadata": {
        "id": "ur-Oiy_EP31J"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "source": [
        "from spatialmath.base import skew, vex\n",
        "\n",
        "skew([1, 2, 3])"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "array([[ 0, -3,  2],\n",
              "       [ 3,  0, -1],\n",
              "       [-2,  1,  0]])"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 6
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "y8i8-6GJ3H2o",
        "outputId": "c6dd3a60-c7ef-4099-e961-786c85952613"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "验证 $\\boldsymbol{R}=\\exp \\left(\\phi^{\\wedge}\\right)$："
      ],
      "metadata": {
        "id": "Tua9IO2oUnqo"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "source": [
        "from spatialmath.base import trexp\n",
        "\n",
        "R1 = SO3.Rx(0.3)\n",
        "trexp([1, 0, 0], 0.3), R1.A"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "(array([[ 1.        ,  0.        ,  0.        ],\n",
              "        [ 0.        ,  0.95533649, -0.29552021],\n",
              "        [ 0.        ,  0.29552021,  0.95533649]]),\n",
              " array([[ 1.        ,  0.        ,  0.        ],\n",
              "        [ 0.        ,  0.95533649, -0.29552021],\n",
              "        [ 0.        ,  0.29552021,  0.95533649]]))"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 7
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "YmTHrgDsUrYn",
        "outputId": "c6430e8e-ad1a-435b-e3c5-3a7885441c92"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "验证 $\\boldsymbol{\\phi} = \\ln(\\boldsymbol{R})^{\\vee}$："
      ],
      "metadata": {
        "id": "SNzGa07AT-xb"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "source": [
        "vex(R1.log()), R1.angvec()"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "(array([0.3, 0. , 0. ]), (0.3, array([1., 0., 0.])))"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 8
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "cWDb8wnfP3Hr",
        "outputId": "24a683fb-46df-47ce-c712-b3a4a399057d"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "### 特殊欧氏群 $SE(3)$\n",
        "同样变换矩阵$\\boldsymbol{T}$构成了特殊欧氏群$SE(3)$：\n",
        "\n",
        "$$\n",
        "S E(3)=\\left\\{\\boldsymbol{T}=\\left[\\begin{array}{cc}\n",
        "\\boldsymbol{R} & \\boldsymbol{t} \\\\\n",
        "\\mathbf{0}^{T} & 1\n",
        "\\end{array}\\right] \\in \\mathbb{R}^{4 \\times 4} \\mid \\boldsymbol{R} \\in S O(3), \\boldsymbol{t} \\in \\mathbb{R}^{3}\\right\\}\n",
        "$$\n",
        "\n",
        "李代数$se(3)$记作$\\boldsymbol{\\xi}$，是一个六维向量。前三维为平移，记作$\\rho$：\n",
        "\n",
        "$$\n",
        "se(3) = \\left\\{\\boldsymbol{\\xi}=\\left[\\begin{array}{c}\n",
        "\\rho \\\\\n",
        "\\phi\n",
        "\\end{array}\\right] \\in \\mathbb{R}^{6}, \\boldsymbol{\\rho} \\in \\mathbb{R}^{3}, \\boldsymbol{\\phi} \\in \\mathfrak{s o}(3), \\boldsymbol{\\xi}^{\\wedge}=\\left[\\begin{array}{ll}\n",
        "\\boldsymbol{\\phi}^{\\wedge} & \\boldsymbol{\\rho} \\\\\n",
        "\\mathbf{0}^{T} & 0\n",
        "\\end{array}\\right] \\in \\mathbb{R}^{4 \\times 4}\\right\\}\n",
        "$$\n",
        "\n",
        "对应的反对称矩阵：\n",
        "\n",
        "$$\n",
        "\\boldsymbol{\\xi}^{\\wedge}=\\left[\\begin{array}{ll}\n",
        "\\boldsymbol{\\phi}^{\\wedge} & \\boldsymbol{\\rho} \\\\\n",
        "\\mathbf{0}^{T} & 0\n",
        "\\end{array}\\right] \\in \\mathbb{R}^{4 \\times 4}\n",
        "$$\n",
        "\n",
        "两者转换公式如下：\n",
        "$$T = \\exp \\left(\\boldsymbol{\\xi}^{\\wedge}\\right)$$\n",
        "\n",
        "$$T^{-1} = \\exp \\left((-\\boldsymbol{\\xi})^{\\wedge}\\right)$$\n",
        "\n",
        "$$\\boldsymbol{\\xi} = \\ln \\left(\\exp \\left(\\boldsymbol{\\xi}^{\\wedge}\\right)\\right)^{\\vee}=\\ln (T)^{\\vee}$$"
      ],
      "metadata": {
        "id": "L0SLLHJ4zqUA"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "source": [
        "from spatialmath import SE3\n",
        "\n",
        "T = SE3.Ry(0.3)\n",
        "T"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "  \u001b[38;5;1m 0.9553  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0.2955  \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m-0.2955  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0.9553  \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 9
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Ykud0PtZU8e8",
        "outputId": "f414df5b-e2b4-4c26-9976-be2b3cf835ac"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "验证$\\boldsymbol{\\xi} = \\ln (T)^{\\vee}$，如下："
      ],
      "metadata": {
        "id": "lahXPwWjplwF"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "source": [
        "from spatialmath.base import vexa\n",
        "\n",
        "vexa(T.log())"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "array([0. , 0. , 0. , 0. , 0.3, 0. ])"
            ]
          },
          "metadata": {
            "tags": []
          },
          "execution_count": 10
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "tPDZJwWY5jeK",
        "outputId": "70c57165-8f03-45fe-ec1c-730da17dd339"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "验证$\\boldsymbol{\\xi}^{\\wedge} = \\ln(T)$，如下："
      ],
      "metadata": {
        "id": "Dtpa70FtrNTd"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "source": [
        "from spatialmath.base import skewa\n",
        "\n",
        "print(skewa([0. , 0. , 0. , 0. , 0.3, 0.]), '\\n')\n",
        "print(T.log())"
      ],
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "[[ 0.  -0.   0.3  0. ]\n",
            " [ 0.   0.  -0.   0. ]\n",
            " [-0.3  0.   0.   0. ]\n",
            " [ 0.   0.   0.   0. ]] \n",
            "\n",
            "[[ 0.   0.   0.3  0. ]\n",
            " [ 0.   0.   0.   0. ]\n",
            " [-0.3  0.   0.   0. ]\n",
            " [ 0.   0.   0.   0. ]]\n"
          ]
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "37P6fQv6qWMJ",
        "outputId": "e599e54b-71f9-412f-c265-da3fb318a0b5"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "验证$T = \\exp \\left(\\boldsymbol{\\xi}^{\\wedge}\\right) = \\exp(\\ln(T))$，如下："
      ],
      "metadata": {
        "id": "Wr49PWEPrsJL"
      }
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "source": [
        "print(T.A, '\\n')\n",
        "print(trexp(T.log()))"
      ],
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "[[ 0.95533649  0.          0.29552021  0.        ]\n",
            " [ 0.          1.          0.          0.        ]\n",
            " [-0.29552021  0.          0.95533649  0.        ]\n",
            " [ 0.          0.          0.          1.        ]] \n",
            "\n",
            "[[ 0.95533649  0.          0.29552021  0.        ]\n",
            " [ 0.          1.          0.          0.        ]\n",
            " [-0.29552021  0.          0.95533649  0.        ]\n",
            " [ 0.          0.          0.          1.        ]]\n"
          ]
        }
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "SdGR5CLjqkv3",
        "outputId": "d3a7a585-3cc7-41ba-87b9-e6a7dbb371da"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "### 旋转表示\n",
        "第$i$帧和第$j$帧之间的相对位姿，在李群$SE(3)$上可以表示为：\n",
        "\n",
        "$$\n",
        "\\boldsymbol{T}_{i,j} = \\boldsymbol{T}_i^{-1}T_j\n",
        "$$\n",
        "\n",
        "也可以在李代数上表示为：\n",
        "\n",
        "$$\n",
        "\\begin{aligned}\n",
        "\\boldsymbol{\\xi}_{i j} &=\\ln \\left(\\boldsymbol{T}_{i}^{-1} \\boldsymbol{T}_{j}\\right)^{\\vee} \\\\\n",
        "&=\\ln \\left(\\exp \\left(\\left(-\\boldsymbol{\\xi}_{i}\\right)^{\\wedge}\\right) \\exp \\left(\\boldsymbol{\\xi}_{j}^{\\wedge}\\right)\\right)^{\\vee}\n",
        "\\end{aligned}\n",
        "$$"
      ],
      "metadata": {
        "id": "r6tawCtPsiYd"
      }
    }
  ]
}