estimate_rotation.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "968e25fd",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ceda565d",
   "metadata": {},
   "source": [
    "# Solve 2D"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8ef9c590",
   "metadata": {},
   "source": [
    "Find 2D rotation matrix R that maps point x onto the line defined by a.x = b"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "id": "18f7485e",
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_matrix(name, nrow, ncol): \n",
    "    return Matrix([[f\"{name}{i}{j}\" for j in range(ncol)] for i in range(nrow)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "id": "22b50925",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "R = make_matrix(\"R\", 2, 2)\n",
    "# x = make_matrix(\"x\", 2, 1)\n",
    "x = Matrix([[0], [1]])\n",
    "a = make_matrix(\"a\", 2, 1)\n",
    "b, = symbols([\"b\"])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "id": "ad69c66a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_{01} a_{00} + R_{11} a_{10} - b$"
      ],
      "text/plain": [
       "R01*a00 + R11*a10 - b"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "((R * x).T * a)[0] -  b"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "id": "af64d0a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "solutions = solve(\n",
    "    [\n",
    "        ((R * x).T * a)[0] -  b, \n",
    "        R * R.T - Identity(2)\n",
    "    ], R, dict=True\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "id": "a7127fa8",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[R00, R01, R10, R11]"
      ]
     },
     "execution_count": 95,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "keys = list(solutions[0].keys())\n",
    "keys"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 91,
   "id": "9c94e2bb",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{\\sqrt{\\frac{a_{00}^{2} a_{10}^{2} + a_{00}^{2} b^{2} - 2 a_{00} a_{10} b \\sqrt{a_{00}^{2} + a_{10}^{2} - b^{2}} + a_{10}^{4} - a_{10}^{2} b^{2}}{a_{00}^{4} + 2 a_{00}^{2} a_{10}^{2} + a_{10}^{4}}} \\left(- a_{00} a_{10} - b \\sqrt{a_{00}^{2} + a_{10}^{2} - b^{2}}\\right)}{a_{10}^{2} - b^{2}}$"
      ],
      "text/plain": [
       "sqrt((a00**2*a10**2 + a00**2*b**2 - 2*a00*a10*b*sqrt(a00**2 + a10**2 - b**2) + a10**4 - a10**2*b**2)/(a00**4 + 2*a00**2*a10**2 + a10**4))*(-a00*a10 - b*sqrt(a00**2 + a10**2 - b**2))/(a10**2 - b**2)"
      ]
     },
     "execution_count": 91,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solutions[0][keys[0]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 92,
   "id": "202b97b8",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{a_{00} b - a_{10} \\sqrt{a_{00}^{2} + a_{10}^{2} - b^{2}}}{a_{00}^{2} + a_{10}^{2}}$"
      ],
      "text/plain": [
       "(a00*b - a10*sqrt(a00**2 + a10**2 - b**2))/(a00**2 + a10**2)"
      ]
     },
     "execution_count": 92,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solutions[0][keys[1]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 93,
   "id": "9986a2f2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle - \\sqrt{\\frac{a_{00}^{2} a_{10}^{2} + a_{00}^{2} b^{2} - 2 a_{00} a_{10} b \\sqrt{a_{00}^{2} + a_{10}^{2} - b^{2}} + a_{10}^{4} - a_{10}^{2} b^{2}}{a_{00}^{4} + 2 a_{00}^{2} a_{10}^{2} + a_{10}^{4}}}$"
      ],
      "text/plain": [
       "-sqrt((a00**2*a10**2 + a00**2*b**2 - 2*a00*a10*b*sqrt(a00**2 + a10**2 - b**2) + a10**4 - a10**2*b**2)/(a00**4 + 2*a00**2*a10**2 + a10**4))"
      ]
     },
     "execution_count": 93,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solutions[0][keys[2]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 94,
   "id": "236318b2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{a_{00} \\sqrt{a_{00}^{2} + a_{10}^{2} - b^{2}}}{a_{00}^{2} + a_{10}^{2}} + \\frac{a_{10} b}{a_{00}^{2} + a_{10}^{2}}$"
      ],
      "text/plain": [
       "a00*sqrt(a00**2 + a10**2 - b**2)/(a00**2 + a10**2) + a10*b/(a00**2 + a10**2)"
      ]
     },
     "execution_count": 94,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solutions[0][keys[3]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a7a748d",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 64,
   "id": "72214644",
   "metadata": {},
   "outputs": [],
   "source": [
    "?cse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "id": "1d4f35df",
   "metadata": {},
   "outputs": [],
   "source": [
    "a, b, x = symbols(['a', 'b'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "270a5d4e",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "5262adc8",
   "metadata": {},
   "source": [
    "# Solve 3D"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "d7251cec",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}r_{00} & r_{10} & r_{20}\\\\r_{01} & r_{11} & r_{21}\\\\r_{02} & r_{12} & r_{22}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[r00, r10, r20],\n",
       "[r01, r11, r21],\n",
       "[r02, r12, r22]])"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "\n",
    "# generate symbols for the matrix elements\n",
    "mat = symbols([[\"r%d%d\" % (i,j) for i in range(3)] for j in range(3)])\n",
    "\n",
    "R = Matrix(mat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "02884bfc",
   "metadata": {},
   "outputs": [],
   "source": [
    "alpha = Matrix(symbols([[\"alpha%d%d\" % (i,j) for i in range(3)] for j in range(3)]))\n",
    "A = Matrix(symbols([[\"a%d%d\" % (i,j) for i in range(3)] for j in range(3)]))\n",
    "Bs = symbols([\"b%d\" % i for i in range(3)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "a952f80a",
   "metadata": {},
   "outputs": [],
   "source": [
    "m0 = alpha[0, :] * R * A[0, :].T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "08b3794a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle a_{00} \\left(\\alpha_{00} r_{00} + \\alpha_{10} r_{01} + \\alpha_{20} r_{02}\\right) + a_{10} \\left(\\alpha_{00} r_{10} + \\alpha_{10} r_{11} + \\alpha_{20} r_{12}\\right) + a_{20} \\left(\\alpha_{00} r_{20} + \\alpha_{10} r_{21} + \\alpha_{20} r_{22}\\right)$"
      ],
      "text/plain": [
       "a00*(alpha00*r00 + alpha10*r01 + alpha20*r02) + a10*(alpha00*r10 + alpha10*r11 + alpha20*r12) + a20*(alpha00*r20 + alpha10*r21 + alpha20*r22)"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solve()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "7a23b75c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\alpha_{00} r_{00} + \\alpha_{10} r_{01} + \\alpha_{20} r_{02} & \\alpha_{00} r_{10} + \\alpha_{10} r_{11} + \\alpha_{20} r_{12} & \\alpha_{00} r_{20} + \\alpha_{10} r_{21} + \\alpha_{20} r_{22}\\\\\\alpha_{01} r_{00} + \\alpha_{11} r_{01} + \\alpha_{21} r_{02} & \\alpha_{01} r_{10} + \\alpha_{11} r_{11} + \\alpha_{21} r_{12} & \\alpha_{01} r_{20} + \\alpha_{11} r_{21} + \\alpha_{21} r_{22}\\\\\\alpha_{02} r_{00} + \\alpha_{12} r_{01} + \\alpha_{22} r_{02} & \\alpha_{02} r_{10} + \\alpha_{12} r_{11} + \\alpha_{22} r_{12} & \\alpha_{02} r_{20} + \\alpha_{12} r_{21} + \\alpha_{22} r_{22}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[alpha00*r00 + alpha10*r01 + alpha20*r02, alpha00*r10 + alpha10*r11 + alpha20*r12, alpha00*r20 + alpha10*r21 + alpha20*r22],\n",
       "[alpha01*r00 + alpha11*r01 + alpha21*r02, alpha01*r10 + alpha11*r11 + alpha21*r12, alpha01*r20 + alpha11*r21 + alpha21*r22],\n",
       "[alpha02*r00 + alpha12*r01 + alpha22*r02, alpha02*r10 + alpha12*r11 + alpha22*r12, alpha02*r20 + alpha12*r21 + alpha22*r22]])"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "alpha * mat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "3dfd3f00",
   "metadata": {},
   "outputs": [],
   "source": [
    "?solve"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dfb59f59",
   "metadata": {},
   "source": [
    "# tests"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "id": "17ba5452",
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "unsupported operand type(s) for ^: 'Symbol' and 'int'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn [34], line 2\u001b[0m\n\u001b[1;32m      1\u001b[0m x, \u001b[38;5;241m=\u001b[39m symbols([\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mx\u001b[39m\u001b[38;5;124m\"\u001b[39m])\n\u001b[0;32m----> 2\u001b[0m solve(\u001b[43mx\u001b[49m\u001b[38;5;241;43m^\u001b[39;49m\u001b[38;5;241;43m2\u001b[39;49m \u001b[38;5;241m==\u001b[39m \u001b[38;5;241m1\u001b[39m, x)\n",
      "\u001b[0;31mTypeError\u001b[0m: unsupported operand type(s) for ^: 'Symbol' and 'int'"
     ]
    }
   ],
   "source": [
    "x, = symbols([\"x\"])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "id": "0e6ae183",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[{x: -1}, {x: 1}]"
      ]
     },
     "execution_count": 39,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solve([x ** 2 - 1], x, dict=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "id": "67db07a9",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "569d0343",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}