testvalidation.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "bdda2b6a",
   "metadata": {},
   "outputs": [],
   "source": [
    "from validation.pnp import camera_pos_from_output\n",
    "from data.worldpos import pixel_to_world_pos_tensor"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "af7aa5b9",
   "metadata": {},
   "outputs": [],
   "source": [
    "import PIL.Image\n",
    "import numpy as np\n",
    "import torch\n",
    "from pathlib import Path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "306bbae8",
   "metadata": {},
   "outputs": [],
   "source": [
    "dimage = PIL.Image.open('chessd.png')\n",
    "ndimage = np.array(dimage, dtype=np.uint8)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "d7f59513",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_to_world = np.loadtxt('chesst.txt')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "3b6c915b",
   "metadata": {},
   "outputs": [],
   "source": [
    "world_tens = pixel_to_world_pos_tensor(torch.from_numpy(ndimage), camera_to_world, (585, 585), (320, 240))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "7b28619e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "torch.Size([120, 160, 3])"
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "world_tens.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "outputs": [],
   "source": [
    "rm, tm = camera_pos_from_output(world_tens.numpy())"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "outputs": [],
   "source": [
    "import cv2"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "outputs": [
    {
     "data": {
      "text/plain": "'4.0.1'"
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cv2.__version__"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "outputs": [],
   "source": [
    "world_pos = world_tens.numpy()"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "outputs": [
    {
     "data": {
      "text/plain": "array([[ 0.        ,  0.        ,  0.        ],\n       [ 0.02130733,  0.12287685, -0.61505166],\n       [ 0.02634493,  0.1519281 , -0.76046572],\n       ...,\n       [ 0.03721001,  0.19773327, -0.99227603],\n       [ 0.03592406,  0.19605187, -0.99218169],\n       [ 0.0359196 ,  0.19606102, -0.99219344]])"
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "world_pos.reshape(-1, 3)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "outputs": [],
   "source": [
    "from data.worldpos import product2\n",
    "\n",
    "h, w, *_channel = world_pos.shape\n",
    "scale = 4\n",
    "hr = range(0, h * scale, scale)\n",
    "wr = range(0, w * scale, scale)\n",
    "axis = product2(hr, wr)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "outputs": [
    {
     "data": {
      "text/plain": "array([[  0.,   0.],\n       [  0.,   4.],\n       [  0.,   8.],\n       ...,\n       [476., 628.],\n       [476., 632.],\n       [476., 636.]], dtype=float32)"
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "axis.astype(np.float32)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "outputs": [],
   "source": [
    "import transforms3d.quaternions as tsq"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "outputs": [
    {
     "data": {
      "text/plain": "(array([[ 0.89986956, -0.12731672, -0.4171633 ],\n        [-0.05376096,  0.91677665, -0.3957655 ],\n        [ 0.43283314,  0.37856442,  0.81813474]]),\n array([[ 0.00692928, -0.20797953,  0.07842185, -0.21835957, -0.39404273,\n         -0.88312387, -0.04152793,  0.88431423, -0.38721646],\n        [ 0.43126374,  0.18732604,  0.87311461,  0.19872041, -0.00632205,\n         -0.04163907, -0.87192425,  0.0783107 ,  0.42505476],\n        [-0.03732008, -0.93726679,  0.20554668,  0.93845715, -0.03743122,\n         -0.21418851,  0.19415231, -0.22456855,  0.00119553]]))"
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cv2.Rodrigues(rm)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "outputs": [],
   "source": [
    "from validation.pnp import  combine_rotation_translation, homo_transform"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "outputs": [
    {
     "data": {
      "text/plain": "array([[ 0.89986956, -0.12731672, -0.4171633 , -0.60954383],\n       [-0.05376096,  0.91677665, -0.3957655 , -0.78611842],\n       [ 0.43283314,  0.37856442,  0.81813474,  0.19616935],\n       [ 0.        ,  0.        ,  0.        ,  1.        ]])"
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "combine_rotation_translation(rm, tm)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "outputs": [],
   "source": [
    "from scipy.spatial.transform.rotation import Rotation"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "outputs": [],
   "source": [
    "rot = Rotation.from_rotvec(rm.squeeze())"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "outputs": [
    {
     "data": {
      "text/plain": "array([[ 0.89986956, -0.12731672, -0.4171633 ],\n       [-0.05376096,  0.91677665, -0.3957655 ],\n       [ 0.43283314,  0.37856442,  0.81813474]])"
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "rot.as_matrix()"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "outputs": [],
   "source": [
    "def homo_translation(translation: np.ndarray):\n",
    "    id_mat = np.identity(4)\n",
    "    id_mat.T[3, :-1] = translation\n",
    "    return id_mat"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "outputs": [],
   "source": [
    "def homo_transform(transform: np.ndarray):\n",
    "    id_mat = np.identity(4)\n",
    "    id_mat[:-1, :-1] = transform\n",
    "    return id_mat#%%\n",
    "\n",
    "est = homo_transform(cv2.Rodrigues(rm)[0])"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "outputs": [],
   "source": [
    "def trans(p, mat):\n",
    "    x, y, z = p\n",
    "    nx, ny, nz, w = np.matmul([x, y, z, 1], mat)\n",
    "    return [nx / w, ny / w, nz / w]"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "outputs": [
    {
     "data": {
      "text/plain": "[-0.7907246719684641, -0.6279393578669117, -0.977138492119077]"
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "trans([1, 1, 1], camera_to_world)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "outputs": [
    {
     "data": {
      "text/plain": "[748.1809218369307, 683.2942508781705, 689.592411407295]"
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "trans([1, 1, 1], inest)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "outputs": [],
   "source": [
    "from validation.projection import default_intrinsics_matrix"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "outputs": [],
   "source": [
    "import numpy.linalg as L"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "outputs": [],
   "source": [
    "inest = est @ (homo_transform(default_intrinsics_matrix))"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "outputs": [
    {
     "data": {
      "text/plain": "array([[526.42369501, -74.48027857, 256.9850855 ,   0.        ],\n       [-31.45016004, 536.31434208, 202.42712491,   0.        ],\n       [253.20738686, 221.46018737, 230.180201  ,   0.        ],\n       [  0.        ,   0.        ,   0.        ,   1.        ]])"
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "inest\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "name": "python3",
   "language": "python",
   "display_name": "Python 3"
  },
  "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
}