{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Four points to a bird’s-eye view\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lecture 11. Put `homography_dlt.py` in the same folder as this notebook, then run the cells in order."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each dark box below is a code cell. Edit it, then press **Run** or **Shift+Enter**. Cells share variables from top to bottom, like Jupyter."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from homography_dlt import camera_homography, apply_h, dlt, check_error_cm, STALL, CHECK\n",
    "\n",
    "H_true = camera_homography()          # ground (X, Y in meters) to pixels\n",
    "np.round(H_true / H_true[2, 2], 3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Where the stall shows up in the image"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "clicks = apply_h(H_true, STALL)\n",
    "for g, c in zip(STALL, clicks):\n",
    "    print(f'ground {g} m  ->  pixel {c.round(1)}')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Fit H back from the four clicks\n",
    "With perfect clicks the DLT recovers H exactly."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "H = dlt(STALL, clicks)\n",
    "print(np.round(H, 3))\n",
    "print('check-point error:', round(check_error_cm(H), 6), 'cm')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Noisy clicks\n",
    "Add 1 pixel of random error to each click and fit again. Run the cell a few times."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "rng = np.random.default_rng()\n",
    "noisy = clicks + rng.normal(0, 1.0, clicks.shape)\n",
    "H_noisy = dlt(STALL, noisy)\n",
    "print('check-point error:', round(check_error_cm(H_noisy), 1), 'cm')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Use more points\n",
    "Fit from all the stall corners in the lot, each with the same 1 px noise."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "grid = np.array([[x, y] for x in (4.5, 10.0, 15.5, 21.0) for y in (-3.9, -1.3, 1.3, 3.9, 6.5)])\n",
    "img = apply_h(H_true, grid) + rng.normal(0, 1.0, (len(grid), 2))\n",
    "H_many = dlt(grid, img)\n",
    "print(len(grid), 'points: check-point error', round(check_error_cm(H_many), 1), 'cm')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 5. Your turn\n",
    "Photograph a printed checkerboard (or floor tiles) with your phone, read the pixel positions of four or more corners from the photo, and put them in `my_pixels` with their true positions in `my_ground` (in meters or squares). Then measure a length you can check with a ruler."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "my_ground = np.array([[0, 0], [0.2, 0], [0.2, 0.2], [0, 0.2]])            # meters on the paper\n",
    "my_pixels = np.array([[100, 400], [520, 380], [560, 90], [140, 70]])       # pixels in your photo\n",
    "H_me = dlt(my_ground, my_pixels)\n",
    "print(np.round(H_me, 3))\n",
    "# where does pixel (330, 230) land on the paper?\n",
    "print(apply_h(np.linalg.inv(H_me), [[330, 230]]).round(3))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}