{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Calibrate a virtual camera\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lecture 14. Put `calib_zhang.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 calib_zhang import make_views, homography, zhang_closed_form, poses_from_k, reprojection_rms, refine, BOARD, TRUE_K, SPEC_K\n",
    "\n",
    "views = make_views(n=12, tilt_deg=30, noise_px=0.3)\n",
    "print(len(views), 'photos,', len(views[0]), 'corners each')\n",
    "print('spec sheet:', SPEC_K)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. One homography per view\n",
    "The board is flat, so each photo is a homography from board meters to pixels: the DLT from Lecture 12."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "H0 = homography(BOARD, views[0])\n",
    "np.round(H0, 3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. The closed form\n",
    "Two equations per view in ω = K⁻ᵀK⁻¹, stacked and solved with the SVD."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "K, Hs = zhang_closed_form(views)\n",
    "np.round(K, 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Reprojection error, then refinement"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "poses = poses_from_k(K, Hs)\n",
    "print(f'closed form RMS: {reprojection_rms(K, poses, views):.3f} px')\n",
    "K2, poses2 = refine(K, poses, views)\n",
    "print(f'refined RMS:     {reprojection_rms(K2, poses2, views):.3f} px')\n",
    "print('refined K:', np.round([K2[0, 0], K2[1, 1], K2[0, 2], K2[1, 2]], 1), ' true:', list(TRUE_K.values()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Your turn: low error, wrong K\n",
    "Calibrate with almost no tilt. Compare the reprojection error with how far K is from the truth."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "for tilt in (30, 15, 5, 2):\n",
    "    v = make_views(n=12, tilt_deg=tilt, noise_px=0.3)\n",
    "    K, Hs = zhang_closed_form(v)\n",
    "    if K is None:\n",
    "        print(f'tilt {tilt:2d}: no answer'); continue\n",
    "    p = poses_from_k(K, Hs)\n",
    "    print(f'tilt {tilt:2d}: RMS {reprojection_rms(K, p, v):.2f} px, fx off by {K[0, 0] - TRUE_K[\"fx\"]:+.0f} px')"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}