{ "cells": [ { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "%matplotlib inline" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n# Going between DLT <-> intrinsic/extrinsic: experimental data\nHere we'll go through using `experimentally derived` DLT coefficients. \nThe DLT coefficients were derived using the `easyWand `_\ntool. Three TeAx ThermalCapture thermal cameras were placed in a cave to record\nbat activity. Here, we'll be tracking a falling object and checking to see \nif our measured gravitational accelaration matches the expected 9.8 $m/s^{2}$. \n\n\n## References\n* DLT to/from intrinsic+extrinsic https://biomech.web.unc.edu/dlt-to-from-intrinsic-extrinsic/ acc 2022-03-08\n* http://www.kwon3d.com/theory/dlt/dlt.html#3d\n\n\nAuthor: Thejasvi Beleyur, March 2022\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "import glob\nimport cv2\nimport matplotlib.pyplot as plt\nfrom mpl_toolkits.mplot3d import Axes3D\nimport pandas as pd\nimport numpy as np \nfrom track2trajectory.camera import Camera\nfrom dlt_reconstruction_easyWand import dlt_reconstruct\nfrom track2trajectory.synthetic_data import make_rotation_mat_fromworld, get_cam_centre_from_projectionmat\nfrom track2trajectory.dlt_to_world import transformation_matrix_from_dlt, cam_centre_from_dlt\nimport mat73\nfrom scipy.spatial import distance\nfrom gravity_alignment import smooth_and_acc, row_calc_norm\n# sphinx_gallery_thumbnail_path = '_static//threed_coordinates.png'\nconcat = np.concatenate\n\ndef point2point_matrix(xyz):\n distmat = np.zeros((xyz.shape[0],xyz.shape[0]))\n for i in range(distmat.shape[0]):\n for j in range(distmat.shape[1]):\n distmat[i,j] = distance.euclidean(xyz[i,:], xyz[j,:])\n return distmat" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Undistorting pixel coordinates\nMost, if not all, cameras have some form of distortion. Once we know the distortion\ncoefficients, it is easy to 'undistort' an image or an object's pixel coordinates,\nand thus generate corrected object coordinates. \n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "# load the raw 2D tracking data\nmulticam_xy = pd.read_csv('DLTdv7_data_gravityxypts.csv')\ncolnames = multicam_xy.columns\ncam1_2d, cam2_2d, cam3_2d = [multicam_xy.loc[:,colnames[each]].to_numpy(dtype='float32') for each in [[0,1], [2,3], [4,5]]]\n\n# the pixel data was digitised using DLTdv (https://biomech.web.unc.edu/dltdv/)\n# The image origin in DLTdv is set to the lower left. DO NOT shift them - or the DLT\n# coefficients won't make sense anymore!!!! \n\ncam1_2d[:,1] = cam1_2d[:,1] # edited legacy code. Previously this hadd 511 - cam1_2d[:,1]\ncam2_2d[:,1] = cam2_2d[:,1] \ncam3_2d[:,1] = cam3_2d[:,1]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "know the intrinsic matrix (common to all cameras)\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "# camera image is 640 x 512\npx,py = 320, 256\nfx, fy = 526, 526 # in pixels\n\nKteax = np.array([[fx, 0, px],\n [0, fy, py],\n [0, 0, 1]])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "same undistortion to all of them. \n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "p1, p2 = np.float32([0,0]) # tangential distortion\nk1, k2, k3 = np.float32([-0.3069, 0.1134, 0]) # radial distortion\ndist_coefs = np.array([k1, k2, p1, p2, k3]) #in the opencv format\n\n# apply undistortion now\ncam1_undist = cv2.undistortPoints(cam1_2d, Kteax, dist_coefs, P=Kteax)\ncam2_undist = cv2.undistortPoints(cam2_2d, Kteax, dist_coefs, P=Kteax)\ncam3_undist = cv2.undistortPoints(cam3_2d, Kteax, dist_coefs, P=Kteax)" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "plt.figure()\nplt.subplot(311)\nplt.plot(cam1_2d[:,0], cam1_2d[:,1],'*')\nplt.plot(cam1_undist[:,:,0], cam1_undist[:,:,1],'^')\nplt.ylim(0,512)\nplt.xlim(0,640)\n\nplt.subplot(312)\nplt.plot(cam2_2d[:,0], cam2_2d[:,1],'*')\nplt.plot(cam2_undist[:,:,0], cam2_undist[:,:,1],'^')\nplt.ylim(0,512)\nplt.xlim(0,640)\n\nplt.subplot(313)\nplt.plot(cam3_2d[:,0], cam3_2d[:,1],'*')\nplt.plot(cam3_undist[:,:,0], cam3_undist[:,:,1],'^')\nplt.ylim(0,512)\nplt.xlim(0,640)\n\nplt.savefig('../docs/source/_static/undistorted_cameracoods.png')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## DLT reconstruction using 11 parameter DLT from easyWand\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "fname = '2018-08-17_wand_dvProject.mat'\n\ndata_dict = mat73.loadmat(fname)\ndltcoefs = data_dict['udExport']['data']['dltcoef']\n\nc1_dlt, c2_dlt, c3_dlt = [dltcoefs[:,col] for col in [0,1,2]]\n\n\ncoefficents = np.column_stack((c1_dlt, c2_dlt))\nxyz_easywand = []\nfor c1_pt, c2_pt in zip(cam1_undist[11:24], cam2_undist[11:24]):\n points = np.append(np.float32(c1_pt), np.float32(c2_pt)).reshape(1,-1)\n xyz_easywanddlt = dlt_reconstruct(coefficents, points)\n xyz_easywand.append(xyz_easywanddlt[0])\nxyz_dlt_easywand = np.array(xyz_easywand).reshape(-1,3)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## DLT -> Transformation-> Projection matrix\nFirst we get the transformation matrix from the DLT coefficients\nand then following the `Ty Hedrick and Taila Weiss write-ups here `_\nwe'll get the final projection matrix $P$.\n\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Handedness is important\nIt is important to know that different packages may have different handed coordinate\nsystems - especially in the rotation matrices. In our case, the output from\n:code:`transfromation_matrix_from_dlt` (a Python port of :code:`DLTcameraPosition`)\ngives a rotation matrix with a right-handed system. We need 'flip' the rotation\nmatrix to get sensible xyz coordinates. \n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "shifter_mat = np.row_stack(([1,0,0,0],\n [0,1,0,0],\n [0,0,-1,0],\n [0,0,0,1]))\n\nT1, _, ypr1 = transformation_matrix_from_dlt(dltcoefs[:,0])\nT2, _, ypr2 = transformation_matrix_from_dlt(dltcoefs[:,1])\nT3, _, ypr3 = transformation_matrix_from_dlt(dltcoefs[:,2])\n\n# shift the whole transformation matrix and then \n# extract the 3x3 rotation matrix out\nshifted_rotmat1 = np.matmul(T1, shifter_mat)[:3,:3]\nshifted_rotmat2 = np.matmul(T2, shifter_mat)[:3,:3]\nshifted_rotmat3 = np.matmul(T3, shifter_mat)[:3,:3]\n\n# Use world camera centre and camera rotation matrix\n# to make world->camera transformation matrix\nT1cam = make_rotation_mat_fromworld(shifted_rotmat1, T1[-1,:3])\nT2cam = make_rotation_mat_fromworld(shifted_rotmat2, T2[-1,:3])\nT3cam = make_rotation_mat_fromworld(shifted_rotmat3, T3[-1,:3])\n\nm = np.column_stack((np.eye(3), np.zeros(3))) # the 3x4 matrix to 'grease the wheels'\n\nP11kmt = np.matmul(Kteax, np.matmul(m,T1cam))\nP22kmt = np.matmul(Kteax, np.matmul(m,T2cam))\nP33kmt = np.matmul(Kteax, np.matmul(m,T3cam))\n\ncam1 = Camera(1, [0,0,0], fx, px, py, fx, fy, Kteax, [0,0,0],\n np.eye(3), np.zeros(5), [0,0,0], P11kmt)\ncam2 = Camera(2, [0,0,0], fx, px, py, fx, fy, Kteax, [0,0,0],\n np.eye(3), np.zeros(5), [0,0,0], P22kmt)\ncam3 = Camera(2, [0,0,0], fx, px, py, fx, fy, Kteax, [0,0,0],\n np.eye(3), np.zeros(5), [0,0,0], P33kmt)\n\nxyz_Tmat_based = []\nfor c1_pt, c2_pt in zip(cam1_undist[11:24], cam2_undist[11:24]):\n position_homog = cv2.triangulatePoints(P11kmt, P22kmt,\n c1_pt.flatten(), c2_pt.flatten())\n xyz_Tmat_based.append(cv2.convertPointsFromHomogeneous(position_homog.T)) \n\nxyz_Tmat_based = np.array(xyz_Tmat_based).reshape(-1,3)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Generate projection matrix from DLT coefficients\nThis is based on my attempts at trying to recreate the projection matrix\n$P$ directly from the DLT coefficients (again inspired by the Hedrick lab link)\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "def extract_P_from_dlt_v2(dltcoefs):\n '''No normalisation\n '''\n dltcoefs = np.append(dltcoefs, 1)\n dltcoefs = dltcoefs\n\n P = dltcoefs.reshape(3,4)\n return P\n\n# generate projection matrix \nPcam1 = extract_P_from_dlt_v2(c1_dlt)\nPcam2 = extract_P_from_dlt_v2(c2_dlt)\n\n# Now get 3D positions using cv2.triangulatePositions\nxyz_P_based = []\nfor pt1, pt2 in zip(cam1_undist[11:24,:], cam2_undist[11:24,:]):\n pt1_homog, pt2_homog = (X.reshape(1,1,2) for X in [pt1, pt2])\n position = cv2.triangulatePoints(Pcam1, Pcam2, pt1_homog, pt2_homog)\n final_xyz = cv2.convertPointsFromHomogeneous(position.T).flatten()\n xyz_P_based.append(final_xyz)\n\nxyz_P_based = np.array(xyz_P_based).reshape(-1,3)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Visualise all of these points - we see the parabolic path of an object that's\nbeen thrown and is falling as it flies through the air.\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "plt.figure(figsize=(6,4))\nax = plt.subplot(311, projection='3d')\nax.view_init(azim=95, elev=-54)\nax.plot(xyz_dlt_easywand[:,0],\n xyz_dlt_easywand[:,2],\n xyz_dlt_easywand[:,1], '*')\nax2 = plt.subplot(312, projection='3d')\nax2.view_init(azim=95, elev=-54)\nax2.plot(xyz_Tmat_based[:,0], xyz_Tmat_based[:,2], xyz_Tmat_based[:,1], '*')\nax3 = plt.subplot(313, projection='3d')\nax3.view_init(azim=95, elev=-54)\nax3.plot(xyz_P_based[:,0], xyz_P_based[:,2], xyz_P_based[:,1], '*')\n\nplt.savefig('../docs/source/_static/threed_coordinates.png')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "How similar or dissilimar are the points. They may have different origins and \naxis orientations - but the euclidean distances between points should remain \nthe same\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "indices = np.arange(13)\npbased_distmat = point2point_matrix(xyz_P_based[indices,:])\ntbased_distmat = point2point_matrix(xyz_Tmat_based[indices,:])\neasywand_distmat = point2point_matrix(xyz_dlt_easywand[indices,:])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Camera centres across the different methods\nDLT based method (code from Ty Hedrick)\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "Ccam1_dltmethod = cam_centre_from_dlt(dltcoefs[:,0])\nCcam2_dltmethod = cam_centre_from_dlt(dltcoefs[:,1])\nintercam_dist_dlt = distance.euclidean(Ccam1_dltmethod, Ccam2_dltmethod)" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "Ccam1_Pmethod = get_cam_centre_from_projectionmat(Pcam1)\nCcam2_Pmethod = get_cam_centre_from_projectionmat(Pcam2)\nintercam_dist_P = distance.euclidean(Ccam1_Pmethod, Ccam2_Pmethod)\n\nprint(f'Inter-camera centre distance: \\n Projection mat: {intercam_dist_P}\\n DLT method: {intercam_dist_dlt}')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Get the overall accelaration. This is a falling object, and so the only\naccelaration should be the gravitational accelaration ~9.8 $m/s^{2}$\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "acc_dlt = smooth_and_acc(xyz_dlt_easywand,fps=25)\nacc_P = smooth_and_acc(xyz_P_based, fps=25)\nacc_tmat = smooth_and_acc(xyz_Tmat_based,fps=25)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "here (%age of `g`)\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "relative_mean_acc = np.array([np.mean(each)/9.81 for each in [row_calc_norm(acc_dlt), row_calc_norm(acc_P), row_calc_norm(acc_tmat)]])\n\nprint(f'g has been estimated to within {relative_mean_acc*100} \\\n of its true value with the 3 methods')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "using the three methods.\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "plt.figure()\nplt.plot(row_calc_norm(acc_dlt),'-.' ,label='dlt reconstruction')\nplt.plot(row_calc_norm(acc_P), '^', label='Projection matrix based')\nplt.plot(row_calc_norm(acc_tmat), label='projection from T matrix')\nplt.hlines(9.8, 0,10,'k', label='g=9.81 $m/s^{2}$')\nplt.legend();plt.ylim(8,10.5)\n\nplt.savefig('../docs/source/_static/g_acc.png')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "get_range = lambda X: np.max(X) - np.min(X)\n\ndef get_stack_absdiff(XXX):\n return np.apply_along_axis(get_range, 2, XXX)\n\nrows, cols = pbased_distmat.shape\nall_distmats = np.zeros((rows, cols, 3))\nall_distmats[:,:,0] = pbased_distmat\nall_distmats[:,:,1] = tbased_distmat\nall_distmats[:,:,2] = easywand_distmat\n\ndistmat_range = get_stack_absdiff(all_distmats)\n\nprint(f'Max range in distance matrices across \\\n 3 methods: {np.max(distmat_range)}')" ] } ], "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.7.9" } }, "nbformat": 4, "nbformat_minor": 0 }