{ "cells": [ { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "%matplotlib inline" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n# Matching and tracking bats in a cave\nHere let's use some more experimental data to find 2D correspondences, \nand finally 3D paths for 2-3 bats seen flying together in a cave. \n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "import matplotlib.pyplot as plt \nimport mat73\nimport numpy as np \nimport pandas as pd\nfrom track2trajectory.projection import calcFundamentalMatrix\nfrom track2trajectory.match3d import match_2dpoints_to_3dtrajectories\nfrom track2trajectory.match2d import generate_2d_correspondences\nfrom track2trajectory.camera import Camera\nfrom track2trajectory.projection import project_to_2d\nfrom track2trajectory.synthetic_data import make_rotation_mat_fromworld, get_cam_coods\nfrom track2trajectory.dlt_to_world import transformation_matrix_from_dlt, cam_centre_from_dlt\n\n# sphinx_gallery_thumbnail_path = '_static/bats3dtraj.png'\n\nthreecam_xy = pd.read_csv('DLTdv8_data_p000_15000_3camxypts.csv')\n\ndef reformat_dltdv_frame(dltdv_df, camname, camnum):\n '''\n '''\n cam_cols = [dltdv_df.loc[:,column] for column in dltdv_df.columns if camname in column]\n horizontal_concat = pd.concat(cam_cols,1)\n unique_points = np.unique([each.split('_')[0] for each in horizontal_concat.columns])\n \n point_subdfs = []\n for point in unique_points:\n ptx = point + '_'+camname+'_X'\n pty = point + '_'+camname+'_Y'\n subdf = dltdv_df.loc[:,[ptx,pty]]\n subdf = subdf.rename(columns={ptx: \"x\", pty: \"y\"})\n subdf['frame'] = np.arange(dltdv_df.shape[0])\n subdf['oid'] = camname +'-'+point\n subdf['cid'] = camnum\n point_subdfs.append(subdf)\n formatted_df = pd.concat(point_subdfs,axis=0).reset_index(drop=True)\n return formatted_df\n \ncam1_xy = reformat_dltdv_frame(threecam_xy, 'cam1', 1)\ncam2_xy = reformat_dltdv_frame(threecam_xy, 'cam2', 2)\ncam3_xy = reformat_dltdv_frame(threecam_xy, 'cam3', 3)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "camera image is 640 x 512\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "px,py = 320, 256\nfx, fy = 526, 526 # in pixels\n\nKteax = np.array([[fx, 0, px],\n [0, fy, py],\n [0, 0, 1]])\n# distortion coefficients\np1, 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" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "same undistortion to all of them. \n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "fname = '2018-08-17_wand_dvProject.mat'\n\nmatfile = False\nif matfile:\n \n data_dict = mat73.loadmat(fname)\n dltcoefs = data_dict['udExport']['data']['dltcoef']\nelse:\n dltcoefs = pd.read_csv('2018-08-17_wand_dltCoefs.csv', header=None).to_numpy()\n \n\nc1_dlt, c2_dlt, c3_dlt = [dltcoefs[:,col] for col in [0,1,2]]\n\ndef 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)\nPcam3 = extract_P_from_dlt_v2(c3_dlt)\n\n# also get the rotation and translation data - placed in the cam->world frame\n\ndef make_transformation_matrix_from_dlt(dltc):\n '''\n '''\n \n\n t11, z11, _ = transformation_matrix_from_dlt(dltc) \n 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 shifted_rotmat1 = np.matmul(t11, shifter_mat)[:3,:3]\n Tmat = make_rotation_mat_fromworld(shifted_rotmat1, t11[-1,:3])\n return Tmat\n\nT1cam = make_transformation_matrix_from_dlt(c1_dlt)\nT2cam = make_transformation_matrix_from_dlt(c2_dlt)\nT3cam = make_transformation_matrix_from_dlt(c3_dlt)\n\ncam1C = cam_centre_from_dlt(c1_dlt)\ncam2C = cam_centre_from_dlt(c2_dlt)\ncam3C = cam_centre_from_dlt(c3_dlt)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Generate camera objects and also remember to get the relevant rotation and \ntranslation matrices in the camera->world frame. \n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "c_x, c_y, f, f_x, f_y = 320, 256, 526, 526, 526\ncam1 = Camera(1, [0,0,0], 526, c_x, c_y, f_x, f_y, Kteax, T1cam[:3,-1], T1cam[:3,:3],\n dist_coefs, [], Pcam1)\ncam2 = Camera(2, [0,0,0], 526, c_x, c_y, f_x, f_y, Kteax, T2cam[:3,-1], T2cam[:3,:3],\n dist_coefs, [], Pcam2)\ncam3 = Camera(3, [0,0,0], 526, c_x, c_y, f_x, f_y, Kteax, T3cam[:3,-1], T3cam[:3,:3],\n dist_coefs, [], Pcam3)\n\nF13 = calcFundamentalMatrix(cam1, cam3)\nF12 = calcFundamentalMatrix(cam1, cam2)\nF23 = calcFundamentalMatrix(cam2, cam3)" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "threed_matches = match_2dpoints_to_3dtrajectories(cam1, cam2, \n cam1_xy,\n cam2_xy, F12,match_method='3dbackpred')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Compare the readings with the groundtruth known\n\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false }, "outputs": [], "source": [ "df = pd.read_csv('DLTdv8_data_p000_15000_3camxyzpts.csv')\n\npt3d_bycam = threed_matches.groupby('c1_oid')\nplt.figure()\na0 = plt.subplot(111 ,projection='3d')\na0.view_init(azim=108, elev=21)\nfor names in ['cam1-pt1', 'cam1-pt2', 'cam1-pt3']:\n p1 = pt3d_bycam.get_group(names) \n plt.plot(p1['x'],p1['z'],p1['y'],'-.',label=names)\n\nfor i,each in enumerate([cam1C, cam2C, cam3C]):\n a,b,c = each\n plt.plot(a,b,c,'^',label='cam'+str(i))\nplt.legend()\n\nplt.savefig('../docs/source/_static/bats3dtraj.png')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n\n" ] } ], "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 }