{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Cameras Tutorial"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is an introductory walkthrough of the symbolic [cameras package](../api/symforce.cam.html) in symforce."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import symforce.symbolic as sf\n",
    "from symforce.notebook_util import display"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's start by creating a linear camera calibration object:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "linear_camera_cal = sf.LinearCameraCal.symbolic(\"cal\")\n",
    "display(linear_camera_cal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This lets us project/deproject points written in the camera frame as so:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_pixel = sf.V2.symbolic(\"p\")\n",
    "camera_ray, _ = linear_camera_cal.camera_ray_from_pixel(camera_pixel)\n",
    "display(camera_ray)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_point_reprojected, _ = linear_camera_cal.pixel_from_camera_point(\n",
    "    camera_ray,\n",
    ")\n",
    "display(camera_point_reprojected)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using camera calibration objects, we can create cameras with additional parameters, such as an image size:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "linear_camera = sf.Camera(\n",
    "    calibration=sf.LinearCameraCal(\n",
    "        focal_length=(440, 400),\n",
    "        principal_point=(320, 240),\n",
    "    ),\n",
    "    image_size=(640, 480),\n",
    ")\n",
    "display(linear_camera)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, when projecting points into the image frame, we can check whether the resulting point is in the bounds determined by image_size:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "point_in_FOV = sf.V3(0, 0, 1)\n",
    "point_outside_FOV = sf.V3(100, 0, 1)\n",
    "for point in (point_in_FOV, point_outside_FOV):\n",
    "    pixel, is_valid = linear_camera.pixel_from_camera_point(point)\n",
    "    print(\n",
    "        \"point={} -> pixel={}, is_valid={}\".format(\n",
    "            point.to_storage(),\n",
    "            pixel.to_storage(),\n",
    "            is_valid,\n",
    "        )\n",
    "    )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also create a camera with a given pose:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "linear_posed_camera = sf.PosedCamera(\n",
    "    pose=sf.Pose3(\n",
    "        # camera is spun 180 degrees about y-axis\n",
    "        R=sf.Rot3.from_yaw_pitch_roll(0, sf.pi, 0),\n",
    "        t=sf.V3(),\n",
    "    ),\n",
    "    calibration=linear_camera.calibration,\n",
    "    image_size=(640, 480),\n",
    ")\n",
    "display(linear_posed_camera)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The given pose can be used to transform points between a global frame and the image frame:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "global_point = sf.V3(0, 0, -1)\n",
    "print(\n",
    "    \"point in global coordinates={} (in camera coordinates={})\".format(\n",
    "        global_point.to_storage(),\n",
    "        (linear_posed_camera.pose * global_point).to_storage(),\n",
    "    )\n",
    ")\n",
    "\n",
    "pixel, is_valid = linear_posed_camera.pixel_from_global_point(global_point)\n",
    "print(\n",
    "    \"global_point={} -> pixel={}, is_valid={}\".format(\n",
    "        global_point.to_storage(), pixel.to_storage(), is_valid\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also transform points in pixel coordinates back into the global frame (given a range):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "range_to_point = (global_point - linear_posed_camera.pose.t).norm()\n",
    "global_point_reprojected, is_valid = linear_posed_camera.global_point_from_pixel(\n",
    "    pixel, range_to_point=range_to_point\n",
    ")\n",
    "display(global_point_reprojected)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we can warp points between two posed cameras given the location of the pixel in the source camera, the inverse range to the point, and the target camera to warp the point into."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Perturb second camera slightly from first (small angular change in roll)\n",
    "perturbed_rotation = linear_posed_camera.pose.R * sf.Rot3.from_yaw_pitch_roll(0, 0, 0.5)\n",
    "target_posed_cam = sf.PosedCamera(\n",
    "    pose=sf.Pose3(R=perturbed_rotation, t=sf.V3()),\n",
    "    calibration=linear_camera.calibration,\n",
    ")\n",
    "# Warp pixel from source camera into target camera given inverse range\n",
    "target_pixel, is_valid = linear_posed_camera.warp_pixel(\n",
    "    pixel=sf.V2(320, 240),\n",
    "    inverse_range=1.0,\n",
    "    target_cam=target_posed_cam,\n",
    ")\n",
    "display(target_pixel)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the examples above we used a linear calibration, but we can use other types of calibrations as well:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "atan_cam = sf.ATANCameraCal(\n",
    "    focal_length=[380.0, 380.0],\n",
    "    principal_point=[320.0, 240.0],\n",
    "    omega=0.35,\n",
    ")\n",
    "camera_ray, is_valid = atan_cam.camera_ray_from_pixel(sf.V2(50.0, 50.0))\n",
    "display(camera_ray)\n",
    "pixel, is_valid = atan_cam.pixel_from_camera_point(camera_ray)\n",
    "display(pixel)"
   ]
  }
 ],
 "metadata": {
  "file_extension": ".py",
  "kernelspec": {
   "display_name": "Python 3",
   "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.8.2"
  },
  "mimetype": "text/x-python",
  "name": "python",
  "npconvert_exporter": "python",
  "pygments_lexer": "ipython2",
  "version": 2
 },
 "nbformat": 4,
 "nbformat_minor": 2
}