3D lidar to camera projection-checkpoint.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "886fcb9b",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import math as m"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "id": "cbb9bba6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# INTRINSIC Matrix K (3X4) that transfers 3D world to 2D image\n",
    "f_x = 1\n",
    "f_y = 1\n",
    "o_x = 0\n",
    "o_y = 0\n",
    "# use 3X3 K to find inverse easier\n",
    "K3x4 = np.array([[f_x, 0, o_x, 0],\n",
    "             [0, f_y, o_y, 0],\n",
    "             [0, 0, 1, 0]])\n",
    "\n",
    "K = np.array([[f_x, 0, o_x],\n",
    "             [0, f_y, o_y],\n",
    "             [0, 0, 1]])\n",
    "K_inv = np.linalg.inv(K)\n",
    "# homogeneous constant s\n",
    "s = 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "10ee3fe1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# enter u, v, X, Y, Z training data\n",
    "u1 = 1.1144839253299392; v1 = -0.4542967786428282; X1 = 1; Y1 = 1; Z1 = 1\n",
    "u2 = 1.495323086326796; v2 = -0.3740196780307018; X2 = 2; Y2 = 3; Z2 = 5\n",
    "u3 = 2.228258883884451; v3 = 0.7771941934107853; X3 = 17; Y3 = 29; Z3 = 22\n",
    "u4 = 1.0942470792579986; v4 = -0.6034516994555602; X4 = -1; Y4 = 4; Z4 = 12\n",
    "u5 = -34.77096256972605; v5 = -17.472373028292107; X5 = -10; Y5 = -10; Z5 = -10\n",
    "u6 = 1.5666747467737108; v6 = 0.9984988011991136; X6 = 100; Y6 = 290; Z6 = 149\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "e6d31f65",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Projection Matrix P0:  [[-0.13693064 -0.06268371 -0.1652899  -0.4472136 ]\n",
      " [-0.07905694 -0.1652899   0.12817663  0.4472136 ]\n",
      " [ 0.15811388 -0.13693064 -0.07905694 -0.67082039]]\n"
     ]
    }
   ],
   "source": [
    "# 02/12/2023: this method is successful as tested in later code box: when given 6 pairs of training data, \n",
    "# we are able to retrive the projection matrix, which estimates the changes in 3D rotation and in translation, \n",
    "# similar to the real 3X4 P matrix\n",
    "\n",
    "# method 1: use 6 pairs of training data (svd) to get 12 unknowns: R11-R33, t1, t2, t3\n",
    "# M = KP. \n",
    "# P = [[R, t], [0, 1]], 4X4 matrix\n",
    "# s[u, v, 1]^T = M[X, Y, Z, 1]^T = KP[X, Y, Z, 1]^T\n",
    "A0 = np.array([[X1, Y1, Z1, 1, 0, 0, 0, 0, -X1*u1, -Y1*u1, -Z1*u1, -1*u1],\n",
    "             [0, 0, 0, 0, X1, Y1, Z1, 1, -X1*v1, -Y1*v1, -Z1*v1, -1*v1],\n",
    "             [X2, Y2, Z2, 1, 0, 0, 0, 0, -X2*u2, -Y2*u2, -Z2*u2, -1*u2],\n",
    "             [0, 0, 0, 0, X2, Y2, Z2, 1, -X2*v2, -Y2*v2, -Z2*v2, -1*v2],\n",
    "             [X3, Y3, Z3, 1, 0, 0, 0, 0, -X3*u3, -Y3*u3, -Z3*u3, -1*u3],\n",
    "             [0, 0, 0, 0, X3, Y3, Z3, 1, -X3*v3, -Y3*v3, -Z3*v3, -1*v3],\n",
    "             [X4, Y4, Z4, 1, 0, 0, 0, 0, -X4*u4, -Y4*u4, -Z4*u4, -1*u4],\n",
    "             [0, 0, 0, 0, X4, Y4, Z4, 1, -X4*v4, -Y4*v4, -Z4*v4, -1*v4],\n",
    "             [X5, Y5, Z5, 1, 0, 0, 0, 0, -X5*u5, -Y5*u5, -Z5*u5, -1*u5],\n",
    "             [0, 0, 0, 0, X5, Y5, Z5, 1, -X5*v5, -Y5*v5, -Z5*v5, -1*v5],\n",
    "             [X6, Y6, Z6, 1, 0, 0, 0, 0, -X6*u6, -Y6*u6, -Z6*u6, -1*u6],\n",
    "             [0, 0, 0, 0, X6, Y6, Z6, 1, -X6*v6, -Y6*v6, -Z6*v6, -1*v6]])\n",
    "\n",
    "m_v0 = np.linalg.svd(A0)[2][11]\n",
    "\n",
    "M0 = np.array([[m_v0[0], m_v0[1], m_v0[2], m_v0[3]],\n",
    "             [m_v0[4], m_v0[5], m_v0[6], m_v0[7]],\n",
    "             [m_v0[8], m_v0[9], m_v0[10], m_v0[11]]])\n",
    "\n",
    "P0 = np.dot(np.linalg.inv(K), M0)\n",
    "\n",
    "print(\"Projection Matrix P0: \", P0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "id": "5794db47",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "row:  1.5707963267948974  pitch:  0.4669759821764231  yaw:  1.5707963267948943\n",
      "Projection Matrix P1:  [[ 2.03738763e-15  1.85413054e-15  1.00000000e+00 -1.27962446e-16]\n",
      " [ 8.92933746e-01  4.50188100e-01 -2.65395967e-15 -4.48461925e-01]\n",
      " [-4.50188100e-01  8.92933746e-01 -7.38408061e-16 -1.13055971e-15]]\n"
     ]
    }
   ],
   "source": [
    "# 02/12/2023: THIS METHOD IS CURRENTLY NOT SUCCESSFUL\n",
    "# method 2: use only 3 pairs of training data to get 6 unknowns: theta, phi, psi, t1, t2, t3\n",
    "# [a, b, c, 1]^T = P*[X, Y, Z, 1]^T\n",
    "# [a, b, c, 1]^T = K^-1 * s * [u, v, 1]^T\n",
    "\n",
    "# translation vector t = [t1, t2, t3]^T\n",
    "# [a, b, c]^T = R*[X, Y, Z]^T + t = R_x * R_y * R_z*[X, Y, Z]^T + t = eig_x * eig_y * eig_z*[X, Y, Z]^T + t\n",
    "# R_x will only rotate [X, Y, Z]^T in the Y and Z axis, so for example, R_x * X = X , R_y * b = b\n",
    "\n",
    "a1, b1, c1 = np.dot(np.linalg.inv(K)*s, [u1, v1, 1])\n",
    "a2, b2, c2 = np.dot(np.linalg.inv(K)*s, [u2, v2, 1])\n",
    "a3, b3, c3 = np.dot(np.linalg.inv(K)*s, [u3, v3, 1])\n",
    "#print(a1,b1,c1)\n",
    "\n",
    "# CORRECT up to here\n",
    "\n",
    "A1 = np.array([[-a1, 0, 0, X1, 0, 0, 1, 0, 0],\n",
    "              [0, -b1, 0, 0, Y1, 0, 0, 1, 0],\n",
    "              [0 ,0, -b1, 0, 0, Z1, 0, 0, 1],\n",
    "              [-a2, 0, 0, X2, 0, 0, 1, 0, 0],\n",
    "              [0, -b2, 0, 0, Y2, 0, 0, 1, 0],\n",
    "              [0, 0, -c2, 0, 0, Z2, 0, 0, 1],\n",
    "              [-a3, 0, 0, X3, 0, 0, 1, 0, 0],\n",
    "              [0, -b3, 0, 0, Y3, 0, 0, 1, 0],\n",
    "              [0 ,0, -c3, 0, 0, Z3, 0, 0, 1]])\n",
    "\n",
    "#ERROR: REDO THE MATRIX, figure out some other way... eigenvalues are wrong, so are the t values \n",
    "\n",
    "v1 = np.linalg.svd(A1)[2][8]\n",
    "eig_x, eig_y, eig_z, eig_yz, eig_xz, eig_xy, t1, t2, t3 = v1 # where eig_xy = eig_x * eig_y\n",
    "# we now know the eigenvalues of the individual R matrices, R = V * sig * V^-1, we know the eigenvectors in term of sin/cos angles\n",
    "# we know the eigenvalues of rotational matrices are cos(x) +- isin(x), we will just arccos the real part of the known eigenvalues\n",
    "#print(v1)\n",
    "theta, phi, psi = np.arccos([eig_x.real, eig_y.real, eig_z.real])\n",
    "print(\"row: \", theta, \" pitch: \", phi, \" yaw: \", psi)\n",
    "\n",
    "R_x = [[1, 0, 0], \n",
    "       [0, m.cos(theta), -m.sin(theta)],\n",
    "       [0, m.sin(theta), m.cos(theta)]]\n",
    "\n",
    "R_y = [[m.cos(phi), 0, m.sin(phi)],\n",
    "      [0, 1, 0],\n",
    "      [-m.sin(phi), 0, m.cos(phi)]]\n",
    "\n",
    "R_z = [[m.cos(psi), -m.sin(psi), 0],\n",
    "      [m.sin(psi), m.cos(psi), 0],\n",
    "      [0, 0, 1]]\n",
    "\n",
    "R = np.dot(np.dot(R_z, R_y), R_x)\n",
    "\n",
    "P1 = np.array([[R[0][0], R[0][1], R[0][2], t1],\n",
    "              [R[1][0], R[1][1], R[1][2], t2],\n",
    "              [R[2][0], R[2][1], R[2][2], t3]])\n",
    "\n",
    "print(\"Projection Matrix P1: \", P1)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "5790c4c8",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 100., -100.,  100.])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.dot(R_x, [100,100,100])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca2a5cda",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "f7ab5cfe",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([2, 2, 2])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "d = np.array([[2, 0, 0],\n",
    "      [0, 2, 0],\n",
    "      [0, 0, 2]])\n",
    "e = np.array([1, 1, 1])\n",
    "np.dot(d,e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "id": "d2bb917f",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[1,1,1] projected:  [1.1144839253299392, -0.4542967786428282]\n",
      "[2,3,5] projected:  [1.4953230863267961, -0.37401967803070185]\n",
      "[17,29,22] projected:  [2.228258883884451, 0.7771941934107853]\n",
      "[-1,4,12] projected:  [1.0942470792579986, -0.6034516994555602]\n",
      "[-10,-10,-10] projected:  [-34.77096256972605, -17.472373028292107]\n",
      "[100,290,149] projected:  [1.5666747467737108, 0.9984988011991136]\n"
     ]
    }
   ],
   "source": [
    "# training data calculation, used for input to train the projection matrix\n",
    "\n",
    "theta = m.pi/3\n",
    "phi = m.pi/4\n",
    "psi = m.pi/6\n",
    "\n",
    "R_x = [[1, 0, 0], \n",
    "       [0, m.cos(theta), -m.sin(theta)],\n",
    "       [0, m.sin(theta), m.cos(theta)]]\n",
    "\n",
    "R_y = [[m.cos(phi), 0, m.sin(phi)],\n",
    "      [0, 1, 0],\n",
    "      [-m.sin(phi), 0, m.cos(phi)]]\n",
    "\n",
    "R_z = [[m.cos(psi), -m.sin(psi), 0],\n",
    "      [m.sin(psi), m.cos(psi), 0],\n",
    "      [0, 0, 1]]\n",
    "R = np.dot(np.dot(R_z, R_y), R_x)\n",
    "\n",
    "\n",
    "# roate 60 deg row, 45 deg pitch, 30 deg yaw, transform [2,-2,3]\n",
    "P = [[R[0][0], R[0][1], R[0][2], 2],\n",
    "    [R[1][0], R[1][1], R[1][2], -2],\n",
    "    [R[2][0], R[2][1], R[2][2], 3],\n",
    "    [0, 0, 0, 1]]\n",
    "\n",
    "# converting homogeneous coordinate to cartesian coordinate\n",
    "def toCartes(a):\n",
    "    return [a[0]/a[2], a[1]/a[2]]\n",
    "\n",
    "print(\"[1,1,1] projected: \", toCartes(np.dot(K3x4, np.dot(P, [1,1,1,1]))))\n",
    "print(\"[2,3,5] projected: \", toCartes(np.dot(K3x4, np.dot(P,[2,3,5,1]))))\n",
    "print(\"[17,29,22] projected: \", toCartes(np.dot(K3x4, np.dot(P,[17,29,22,1]))))\n",
    "print(\"[-1,4,12] projected: \", toCartes(np.dot(K3x4, np.dot(P,[-1,4,12,1]))))\n",
    "print(\"[-10,-10,-10] projected: \", toCartes(np.dot(K3x4, np.dot(P,[-10,-10,-10,1]))))\n",
    "print(\"[100,290,149] projected: \", toCartes(np.dot(K3x4, np.dot(P,[100,290,149,1]))))\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "id": "f549cd19",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "P0 result:  [5.7195031606708495, 1.729557015371717]\n",
      "P1 result:  [2.258633168650372, 3.0234904413535824]\n",
      "actual P result:  [5.719503160670909, 1.7295570153717235]\n"
     ]
    }
   ],
   "source": [
    "# test if the projection matrices calculated are accurate \n",
    "\n",
    "print(\"P0 result: \", toCartes(np.dot(P0, [100,100,100,1])))\n",
    "print(\"P1 result: \",toCartes(np.dot(P1, [100,100,100,1])))\n",
    "print(\"actual P result: \", toCartes(np.dot(K3x4, np.dot(P,[100,100,100,1]))))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13f913d9",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}