{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Aim the roof camera\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lecture 11. Put `pinhole_camera.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": [
    "from pinhole_camera import intrinsics, rotation_world_to_camera, project, coverage\n",
    "\n",
    "K = intrinsics(5.0)      # Blackfly S with the 5 mm lens\n",
    "K"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Project one point by hand\n",
    "A point 10 m ahead and 2 m to the left, on the ground. The camera is 1.7 m up and pitched 8 degrees down."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "R = rotation_world_to_camera(8.0)\n",
    "P = np.array([10.0, 2.0, 0.0])\n",
    "pc = R @ (P - np.array([0.0, 0.0, 1.7]))        # into the camera optical frame\n",
    "print('camera frame (x right, y down, z forward):', pc.round(3))\n",
    "uvw = K @ pc\n",
    "print('pixel:', (uvw[:2] / uvw[2]).round(1))\n",
    "print('same with project():', project([P])[0].round(1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Field of view and footprint"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "c = coverage(h=1.7, pitch_deg=8.0, f_mm=5.0)\n",
    "print(f\"f = {c['f_px']:.0f} px, FOV {c['hfov_deg']:.1f} x {c['vfov_deg']:.1f} deg, nearest ground {c['nearest_ground_m']:.2f} m\")\n",
    "for r in c['footprint']:\n",
    "    print(f\"{r['X_m']:4.0f} m: {100*r['lateral_m_per_px']:.1f} cm across, {100*r['along_m_per_px']:.1f} cm along\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Compare lenses\n",
    "Plot the along-the-road pixel size against distance for four lenses."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "X = np.linspace(3, 40, 60)\n",
    "for f_mm in (3.5, 5, 8, 12):\n",
    "    f = f_mm / 0.0045\n",
    "    plt.plot(X, 100 * (X**2 + 1.7**2) / (1.7 * f), label=f'{f_mm} mm')\n",
    "plt.xlabel('distance ahead (m)'); plt.ylabel('one pixel along the road (cm)'); plt.yscale('log'); plt.legend(); plt.grid(True, which='both', alpha=0.3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Your turn\n",
    "Team A: what pitch puts the ground from 3 m to 30 m in view, at the height of the roof bar? Change `h` and try pitches."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "h = 1.7\n",
    "for pitch in (4, 6, 8, 10, 12):\n",
    "    c = coverage(h, pitch, 5.0)\n",
    "    horizon = 'in view' if 0 < c['horizon_row'] < 1240 else 'out of view'\n",
    "    print(f\"pitch {pitch:2d} deg: nearest ground {c['nearest_ground_m']:.1f} m, horizon {horizon}\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}