Work.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from cv2 import normalize\n",
    "import numpy as np\n",
    "import cv2 as cv\n",
    "import math\n",
    "import time\n",
    "import re\n",
    "import os"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def filter_pose_mask(array, mask):\n",
    "    return [element for (i, element) in enumerate(array) if mask[i][0] == 1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "bf = cv.BFMatcher(cv.NORM_L1)\n",
    "\n",
    "def featurematch_relloc(ref_data, query_data, camera_mtx, draw=False):\n",
    "    (ref_img, (ref_kp, ref_desc)) = ref_data\n",
    "    (query_img, (query_kp, query_desc)) = query_data\n",
    "\n",
    "    # Match descriptors.\n",
    "    matches = bf.knnMatch(ref_desc, query_desc, k=2)\n",
    "    \n",
    "    good_matches = []\n",
    "    for m, n in matches:\n",
    "        if m.distance < 0.75 * n.distance:\n",
    "            good_matches.append(m)\n",
    "\n",
    "    if len(good_matches) < 5:\n",
    "        return None, None\n",
    "\n",
    "    good_ref_points = np.zeros(shape=(len(good_matches), 2), dtype=np.float32)\n",
    "    good_query_points = np.zeros(shape=(len(good_matches), 2), dtype=np.float32)\n",
    "    for i in range(len(good_matches)):\n",
    "        match = good_matches[i]\n",
    "        good_ref_points[i, 0] = ref_kp[match.queryIdx].pt[0]\n",
    "        good_ref_points[i, 1] = ref_kp[match.queryIdx].pt[1]\n",
    "        good_query_points[i, 0] = query_kp[match.trainIdx].pt[0]\n",
    "        good_query_points[i, 1] = query_kp[match.trainIdx].pt[1]\n",
    "\n",
    "    E, E_mask = cv.findEssentialMat(good_query_points, good_ref_points, camera_mtx, method=cv.RANSAC, prob=0.99, threshold=0.1, mask=None)\n",
    "    retval, delta_R, delta_t, pose_mask = cv.recoverPose(E, good_query_points, good_ref_points, cameraMatrix=camera_mtx, mask=E_mask)\n",
    "\n",
    "    if draw:\n",
    "        img = cv.drawMatches(ref_img,ref_kp,query_img,query_kp,filter_pose_mask(good_matches, pose_mask),None,flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) \n",
    "        #img = draw_matches(img1, img2, good_old, good_new)\n",
    "        cv.imshow('matches', img)\n",
    "        cv.waitKey()\n",
    "\n",
    "    return delta_t, delta_R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def intersect_rays(ao, ad, bo, bd):\n",
    "    dx = bo[0] - ao[0]\n",
    "    dy = bo[1] - ao[1]\n",
    "    det = bd[0] * ad[1] - bd[1] * ad[0]\n",
    "    u = (dy * bd[0] - dx * bd[1]) / det\n",
    "    v = (dy * ad[0] - dx * ad[1]) / det\n",
    "    \n",
    "    return ao + ad * u"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def unit_vector(vector):\n",
    "    return vector / np.linalg.norm(vector)\n",
    "\n",
    "# v1 and v2 must be normalized\n",
    "def angle_between(v1, v2):\n",
    "    return np.arccos(np.clip(np.dot(v1, v2), -1.0, 1.0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rotate_vector(v, r):\n",
    "    rot = np.array([[math.cos(r), -math.sin(r)], [math.sin(r), math.cos(r)]])\n",
    "    v_rot = np.dot(rot, v)\n",
    "    return v_rot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_direction(frame_with_points, ref_idx, query_idx, angles, mtx, correct_angle=True, draw=False):\n",
    "    t1, r1 = featurematch_relloc(frame_with_points[ref_idx], frame_with_points[query_idx], mtx, draw)\n",
    "    if t1 is None or r1 is None:\n",
    "        return None, None\n",
    "    v = (t1[[0, 2], 0])\n",
    "    if correct_angle:\n",
    "        ref_r = angles[ref_idx]\n",
    "        v_rot = rotate_vector(v, ref_r)\n",
    "        return v_rot, r1\n",
    "    else:\n",
    "        return v, r1\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_average_intersect(rays, positions):\n",
    "    locations = []\n",
    "    for (i1, v1) in enumerate(rays):\n",
    "        for (i2, v2) in enumerate(rays[i1+1:]):\n",
    "            if v1 is None or v2 is None:\n",
    "                continue\n",
    "            \n",
    "            angle = angle_between(v1, v2)\n",
    "            if angle < 0:\n",
    "                angle = 2 * np.pi + angle\n",
    "\n",
    "            p1 = positions[i1]\n",
    "            p2 = positions[i2]\n",
    "            location = intersect_rays(p1, v1, p2, v2)\n",
    "            if np.dot(v1, location - p1) > 0 and np.dot(v2, location - p2) > 0:\n",
    "                locations.append(location)\n",
    "\n",
    "    valid = [location for location in locations if not np.isnan(location).any()]\n",
    "    estimated_location = np.median(valid, axis=0)\n",
    "    return estimated_location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def locate(ref_directions, ref_positions, plot=False, true_position=None):\n",
    "    previous_estimate = None\n",
    "    current_estimate = None\n",
    "    has_data = len(ref_positions) > 0\n",
    "    thresholds = [np.pi / 2, np.pi / 4, np.pi / 8, np.pi / 16]\n",
    "    loop_num = 0\n",
    "    \n",
    "    current_estimate = get_average_intersect(ref_directions, ref_positions)\n",
    "    while has_data and loop_num < len(thresholds):\n",
    "        good_positions = []\n",
    "        good_directions = []\n",
    "        for (i, position) in enumerate(ref_positions):\n",
    "            to_estimate = unit_vector(current_estimate - position)\n",
    "            ref_ray = ref_directions[i]\n",
    "            if ref_ray is not None:\n",
    "                angle = angle_between(to_estimate, ref_ray)\n",
    "                threshold = thresholds[loop_num]\n",
    "                if angle < threshold or angle > (2 * np.pi) - threshold:\n",
    "                    good_positions.append(position)\n",
    "                    good_directions.append(ref_ray)\n",
    "\n",
    "        ref_positions = good_positions\n",
    "        ref_directions = good_directions\n",
    "        loop_num = loop_num + 1\n",
    "        has_data = len(ref_positions) > 0\n",
    "\n",
    "        if not has_data:\n",
    "            return current_estimate\n",
    "\n",
    "        previous_estimate = current_estimate\n",
    "        current_estimate = get_average_intersect(ref_directions, ref_positions)\n",
    "        if np.isnan(current_estimate).any():\n",
    "            return previous_estimate\n",
    "\n",
    "        if plot:\n",
    "            print(current_estimate)\n",
    "            plot_rays(ref_directions, ref_positions, [current_estimate, true_position])\n",
    "\n",
    "    return current_estimate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compass_heading_to_angle(heading):\n",
    "    # North is -135.6 degrees from the world x-axis\n",
    "    # 0 degrees from the camera's perspective is straight forward along the camera y-axis so another 90 degrees to convert to camera x-axis\n",
    "    correction_degrees = 135.6 + 90\n",
    "    correction = np.deg2rad(correction_degrees)\n",
    "    angle = heading - correction\n",
    "    if angle < 0:\n",
    "        angle = 2 * np.pi + angle\n",
    "    return angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def protractor_reading_to_angle(reading):\n",
    "    # Protractor +tive is clock wise on the grid\n",
    "    # Protractor 0 degrees is -90 degrees from the world x-axis\n",
    "    # 0 degrees from the camera's perspective is straight forward along the camera y-axis so another 90 degrees to convert to camera x-axis\n",
    "    reading = -reading\n",
    "    reading = reading - (90 + 90)\n",
    "    return np.deg2rad(reading)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "with np.load('calibration.npz') as X:\n",
    "    mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "base = \"./protractor_positioned/\"\n",
    "paths = os.listdir(base)\n",
    "\n",
    "angles = []\n",
    "tagged_angles = []\n",
    "positions = []\n",
    "for path in paths:\n",
    "    result = re.search(\"x(\\d*\\.\\d*)_y(\\d*\\.\\d*)_ch(\\d*\\.\\d*)_t(-?\\d*\\.?\\d*).png\", path)\n",
    "    if result:\n",
    "        positions.append(np.array([float(result.group(1)), float(result.group(2))]))\n",
    "        angles.append(compass_heading_to_angle(float(result.group(3))))\n",
    "        tagged_angles.append(protractor_reading_to_angle(float(result.group(4))))\n",
    "\n",
    "frames = [cv.cvtColor(cv.imread(base + path), cv.COLOR_BGR2GRAY) for path in paths]\n",
    "\n",
    "first_frame = frames[0]\n",
    "h, w = first_frame.shape[:2]\n",
    "newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))\n",
    "\n",
    "undistorted = [cv.undistort(frame, mtx, dist, None, newcameramtx) for frame in frames]\n",
    "\n",
    "detector = cv.ORB_create(10000)\n",
    "descriptor = cv.xfeatures2d.BEBLID_create(0.75)\n",
    "\n",
    "frame_with_points = [(frame, descriptor.compute(frame, detector.detect(frame, None))) for frame in undistorted]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "def plot_rays(directions, positions, additional_positions=[]):\n",
    "    for (i, pos) in enumerate(positions):\n",
    "        dir = directions[i]\n",
    "        if dir is not None:\n",
    "            dir = dir * 100\n",
    "            x = [pos[0], pos[0] + dir[0]]\n",
    "            y = [pos[1], pos[1] + dir[1]]\n",
    "            plt.plot(x, y, marker='o')\n",
    "            plt.plot(pos[0], pos[1], marker=\"^\", ms=10)\n",
    "\n",
    "    for positions in additional_positions:\n",
    "        plt.plot(positions[0], positions[1], marker='x', ms=15)\n",
    "    plt.xlim(0, 200)\n",
    "    plt.ylim(-20, 200)\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "['x90.0_y60.0_ch1.127_t-22.5.png',\n",
       " 'x90.0_y60.0_ch0.810_t-11.25.png',\n",
       " 'x120.0_y60.0_ch6.259_t22.5.png',\n",
       " 'x90.0_y60.0_ch0.276_t11.25.png',\n",
       " 'x120.0_y60.0_ch0.996_t-11.25.png',\n",
       " 'x90.0_y60.0_ch0.579_t0.png',\n",
       " 'x90.0_y120.0_ch0.184_t22.5.png',\n",
       " 'x120.0_y60.0_ch0.736_t-11.25.png',\n",
       " 'x90.0_y60.0_ch1.109_t-22.5.png',\n",
       " 'x90.0_y120.0_ch1.073_t-22.5.png',\n",
       " 'x120.0_y60.0_ch0.504_t0.png',\n",
       " 'x120.0_y90.0_ch0.790_t-11.25.png',\n",
       " 'x120.0_y60.0_ch0.420_t22.5.png',\n",
       " 'x90.0_y120.0_ch0.849_t-11.25.png',\n",
       " 'x90.0_y60.0_ch0.739_t0.png',\n",
       " 'x120.0_y60.0_ch1.144_t-22.5.png',\n",
       " 'x120.0_y120.0_ch0.903_t-11.25.png',\n",
       " 'x120.0_y90.0_ch1.042_t-22.5.png',\n",
       " 'x120.0_y120.0_ch1.121_t-22.5.png',\n",
       " 'x120.0_y90.0_ch0.259_t11.25.png',\n",
       " 'x120.0_y120.0_ch0.693_t0.png',\n",
       " 'x90.0_y60.0_ch0.921_t-11.25.png',\n",
       " 'x90.0_y60.0_ch6.278_t22.5.png',\n",
       " 'x120.0_y90.0_ch0.496_t0.png',\n",
       " 'x90.0_y120.0_ch0.426_t11.25.png',\n",
       " 'x90.0_y60.0_ch0.522_t11.25.png',\n",
       " 'x90.0_y60.0_ch0.263_t22.5.png',\n",
       " 'x120.0_y60.0_ch0.819_t0.png',\n",
       " 'x120.0_y60.0_ch0.589_t11.25.png',\n",
       " 'x90.0_y120.0_ch0.569_t0.png',\n",
       " 'x120.0_y60.0_ch1.033_t-22.5.png',\n",
       " 'x120.0_y60.0_ch0.219_t11.25.png']"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "paths"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "max_a = protractor_reading_to_angle(0)\n",
    "min_a = protractor_reading_to_angle(12)\n",
    "\n",
    "def location_query(frame_with_points, query, positions, angles, mtx, true_position=None):\n",
    "    refs = [i for i in range(0, len(positions)) if i is not query]  \n",
    "    ref_positions = []\n",
    "    ref_directions = []\n",
    "    ref_rotations = []\n",
    "    for ref in refs:\n",
    "        if angles[ref] > min_a and angles[ref] < max_a:\n",
    "            ref_positions.append(positions[ref])\n",
    "            dir, r = get_direction(frame_with_points, ref, query, angles, mtx, True, False)\n",
    "            ref_directions.append(dir)\n",
    "            ref_rotations.append(r)\n",
    "\n",
    "    return locate(ref_directions, ref_positions, False, true_position)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[90. 60.]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "array([87.69545042, 73.92569312])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "query = 3\n",
    "print(positions[query])\n",
    "location = location_query(frame_with_points, query, positions, tagged_angles, mtx, positions[query])\n",
    "location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 88,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Guessed: [63.67752555 56.27253689] for [90. 60.]. Distance is 26.585083082217178\n",
      "Guessed: [65.77627363 62.59174645] for [90. 60.]. Distance is 24.361979990503453\n",
      "Guessed: [109.10870247  37.15514025] for [120.  60.]. Distance is 25.30825910090893\n",
      "Guessed: [76.2772309  57.07408189] for [90. 60.]. Distance is 14.031229044905361\n",
      "Failed\n",
      "Guessed: [80.15032445 69.09532416] for [90. 60.]. Distance is 13.406753150317966\n",
      "Guessed: [147.77924784 233.44358485] for [ 90. 120.]. Distance is 127.3102054985322\n",
      "Failed\n",
      "Guessed: [70.35399181 64.30724787] for [90. 60.]. Distance is 20.11263339284103\n",
      "Guessed: [101.88447708 134.48684941] for [ 90. 120.]. Distance is 18.73791880823857\n",
      "Guessed: [112.57710471  47.58806192] for [120.  60.]. Distance is 14.462212193040534\n",
      "Guessed: [123.51132334  82.35478414] for [120.  90.]. Distance is 8.413008807317105\n",
      "Guessed: [110.09374023  52.09118472] for [120.  60.]. Distance is 12.676093316851052\n",
      "Guessed: [ 97.48983057 128.26233006] for [ 90. 120.]. Distance is 11.151845585348667\n",
      "Guessed: [81.64246732 59.28194709] for [90. 60.]. Distance is 8.38832239132119\n",
      "Failed\n",
      "Guessed: [126.42816372 110.8617956 ] for [120. 120.]. Distance is 11.17264823019102\n",
      "Failed\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/deaston/.local/share/virtualenvs/RoboPose-93SmMfVE/lib/python3.9/site-packages/numpy/core/fromnumeric.py:3474: RuntimeWarning: Mean of empty slice.\n",
      "  return _methods._mean(a, axis=axis, dtype=dtype,\n",
      "/Users/deaston/.local/share/virtualenvs/RoboPose-93SmMfVE/lib/python3.9/site-packages/numpy/core/_methods.py:189: RuntimeWarning: invalid value encountered in double_scalars\n",
      "  ret = ret.dtype.type(ret / rcount)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Guessed: [135.19352765 175.53150103] for [120. 120.]. Distance is 57.57248377983991\n",
      "Guessed: [126.69301567  83.34390351] for [120.  90.]. Distance is 9.439283829103367\n",
      "Guessed: [128.01403794 117.15876894] for [120. 120.]. Distance is 8.502787661741923\n",
      "Guessed: [75.30133457 59.49614931] for [90. 60.]. Distance is 14.70729855934166\n",
      "Guessed: [41.65789785 56.97965117] for [90. 60.]. Distance is 48.436363890019294\n",
      "Guessed: [118.34793191  89.14905703] for [120.  90.]. Distance is 1.8583414385805939\n",
      "Failed\n",
      "Guessed: [72.95811683 56.54566464] for [90. 60.]. Distance is 17.388450610194063\n",
      "Guessed: [77.04308007 57.58915956] for [90. 60.]. Distance is 13.179299133171641\n",
      "Guessed: [79.32640814 36.12363618] for [120.  60.]. Distance is 47.163776610376694\n",
      "Guessed: [116.54975008  49.5494597 ] for [120.  60.]. Distance is 11.005363108461072\n",
      "Guessed: [ 97.59308716 130.26928724] for [ 90. 120.]. Distance is 12.771579113590326\n",
      "Failed\n",
      "Failed\n"
     ]
    }
   ],
   "source": [
    "compass_distances = []\n",
    "\n",
    "for i in range(0, len(positions)):\n",
    "    try:\n",
    "        query = i\n",
    "        estimate_position = location_query(frame_with_points, query, positions, angles, mtx)\n",
    "        true_position = positions[query]\n",
    "        distance = np.linalg.norm(true_position - estimate_position)\n",
    "        compass_distances.append(distance)\n",
    "        print(f\"Guessed: {estimate_position} for {true_position}. Distance is {distance}\")\n",
    "    except:\n",
    "        print(f\"Failed\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 93,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([  1.32981308,   4.8401543 ,   8.2610557 ,  15.99274894,\n",
       "        87.18202225, 352.48086669])"
      ]
     },
     "execution_count": 93,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.percentile(distances, [10, 50, 70, 80, 90, 100])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "31.18529478527377"
      ]
     },
     "execution_count": 86,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.mean(distances)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 94,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([  8.44892035,  14.03122904,  19.83769048,  25.5636239 ,\n",
       "        47.92732898, 127.3102055 ])"
      ]
     },
     "execution_count": 94,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.percentile(compass_distances, [10, 50, 70, 80, 90, 100])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "23.12572881307819"
      ]
     },
     "execution_count": 95,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "ename": "",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31mThe Kernel crashed while executing code in the the current cell or a previous cell. Please review the code in the cell(s) to identify a possible cause of the failure. Click <a href='https://aka.ms/vscodeJupyterKernelCrash'>here</a> for more info. View Jupyter <a href='command:jupyter.viewOutput'>log</a> for further details."
     ]
    }
   ],
   "source": [
    "np.mean(compass_distances)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "6977848c380f49b6543a9ce90286871c4c839064337da47627b6bf46a88066fb"
  },
  "kernelspec": {
   "display_name": "Python 3.9.12 ('RoboPose-93SmMfVE')",
   "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.9.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}