{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Calibration\n",
    "\n",
    "## 0. Import packages"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "import cv2\n",
    "import matplotlib.pyplot\n",
    "\n",
    "import openalea.phenomenal.data as phm_data\n",
    "import openalea.phenomenal.display as phm_display\n",
    "import openalea.phenomenal.calibration as phm_calib"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Detect chessboard corners in the images\n",
    "\n",
    "### 1.0 Load Images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "chessboard_images = phm_data.chessboard_images(\"data/plant_1\")[0]\n",
    "phm_display.show_image(chessboard_images[\"side\"][42])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.1 Create chessboard object\n",
    "\n",
    "Create chessboard object with square size and shape of chessboard"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "square_size_of_chessboard = 47  # In mm\n",
    "square_shape_of_chessboard = (8, 6)  # (8 square x 6 square on chessboard)\n",
    "\n",
    "# BUILD CHESSBOARD OBJECT\n",
    "chessboard = phm_calib.Chessboard(square_size_of_chessboard, square_shape_of_chessboard)\n",
    "\n",
    "# DISPLAY IT\n",
    "print(chessboard)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.2 Detect corners from images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for id_camera in chessboard_images:\n",
    "    for angle in chessboard_images[id_camera]:\n",
    "        im = chessboard_images[id_camera][angle]\n",
    "        found = chessboard.detect_corners(id_camera, angle, im, check_order=False)\n",
    "        print(\n",
    "            \"Angle {} - Chessboard corners {}\".format(\n",
    "                angle, \"found\" if found else \"not found\"\n",
    "            )\n",
    "        )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.3 Display chessboard corners on images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "angle = 42\n",
    "img = chessboard_images[\"side\"][angle].copy()\n",
    "\n",
    "# DRAW RED POINT ON IMAGE CORNERS POSITION\n",
    "points_2d = chessboard.image_points[id_camera][angle].astype(int)\n",
    "for x, y in zip(points_2d[:, 0, 1], points_2d[:, 0, 0]):\n",
    "    cv2.circle(img, (y, x), 5, (255, 0, 0), -1)\n",
    "\n",
    "phm_display.show_image(img)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.4 Dump & load"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "chessboard.dump(\"chessboard.json\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "chessboard = phm_calib.Chessboard.load(\"chessboard.json\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Multi-camera calibration\n",
    "\n",
    "### 2.0 Load chessboard object with all corners points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load image points\n",
    "image_points = phm_data.image_points(\"data/plant_1\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.1 Do calibration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# setup calibration layout\n",
    "calibrator = phm_calib.Calibrator(\n",
    "    south_camera=(\"side\", 90, 5500),\n",
    "    targets={\"target_1\": (45, 48), \"target_2\": (45, 48 + 180)},\n",
    "    chessboards={\"target_1\": (47, 8, 6), \"target_2\": (47, 8, 6)},\n",
    ")\n",
    "calibrator.load_image_points(image_points)\n",
    "\n",
    "# calibrate\n",
    "calibration = calibrator.calibrate(verbose=False)\n",
    "\n",
    "# Error of reprojection (in pixel distance)\n",
    "print(calibration.calibration_statistics[\"mean_error\"])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.2 Dump"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Dump\n",
    "calibration.dump(\"calibration_camera_side.json\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.6 Viewing calibration result\n",
    "\n",
    "#### 2.6.2 Show chessboard image with corners projection"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "angle = 42\n",
    "circle_radius = 5\n",
    "img = chessboard_images[\"side\"][angle].copy()\n",
    "\n",
    "# RED POINTS ARE POINTS POSITIONS DETECTED BY OPENCV CHESSBOARD DETECTION\n",
    "pt_2d = image_points[\"target_1\"].get_corners_2d(\"side\")[42].astype(int)\n",
    "for x, y in pt_2d:\n",
    "    cv2.circle(img, (x, y), circle_radius, (255, 0, 0), -1)\n",
    "\n",
    "# BLUE POINTS ARE POINTS POSITIONS PROJECTED BY CALIBRATION CHESSBOARD COMPUTATION\n",
    "points_2d = calibration.get_target_projected(\"side\", \"target_1\", 42).astype(int)\n",
    "for x, y in pt_2d:\n",
    "    cv2.circle(img, (x, y), circle_radius, (0, 0, 255), -1)\n",
    "phm_display.show_image(img)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.7"
  },
  "widgets": {
   "application/vnd.jupyter.widget-state+json": {
    "state": {},
    "version_major": 2,
    "version_minor": 0
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}