{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# A ground map from a driving car\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lecture 14. Put `ground_mosaic.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",
    "import matplotlib.pyplot as plt\n",
    "from ground_mosaic import mosaic, pose, LOOP_M\n",
    "\n",
    "img, stats = mosaic()          # no errors\n",
    "print(f'loop {LOOP_M:.0f} m, {stats[\"frames\"]} images')\n",
    "plt.figure(figsize=(9, 6)); plt.imshow(img); plt.axis('off');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Where the car is\n",
    "`pose(s)` gives base_link after s meters: x, y, and heading. RTK sees the antenna, 1 m ahead of it."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "for s in (0, 20, 50, 80):\n",
    "    x, y, h = pose(s)\n",
    "    print(f's = {s:3d} m: x {x:5.1f}, y {y:5.1f}, heading {np.degrees(h):6.1f} deg')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. One error at a time"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "cases = {'heading 0.5 deg': dict(heading_err_deg=0.5), 'time 100 ms': dict(offset_ms=100),\n",
    "         'pitch 0.3 deg': dict(pitch_err_deg=0.3), 'no lever arm': dict(ignore_lever=True)}\n",
    "fig, ax = plt.subplots(2, 2, figsize=(12, 8))\n",
    "for a, (name, kw) in zip(ax.ravel(), cases.items()):\n",
    "    im, st = mosaic(**kw)\n",
    "    a.imshow(im); a.set_title(f'{name}: {100 * st[\"err_10m\"]:.0f} cm at 10 m'); a.axis('off')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Your turn\n",
    "A time offset grows with speed. How fast can the car drive before a 30 ms offset moves points more than 10 cm?"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "for v in (2, 4, 6, 8, 10):\n",
    "    _, st = mosaic(speed=v, offset_ms=30)\n",
    "    print(f'{v:2d} m/s: {100 * st[\"err_10m\"]:5.1f} cm at 10 m ahead')"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}