From 3d364bd237d69f7bf7f223068ba4604208ad8d3d Mon Sep 17 00:00:00 2001 From: crosstyan Date: Thu, 5 Dec 2024 18:52:43 +0800 Subject: [PATCH] idk --- play.ipynb | 212 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100644 play.ipynb diff --git a/play.ipynb b/play.ipynb new file mode 100644 index 0000000..0e7d203 --- /dev/null +++ b/play.ipynb @@ -0,0 +1,212 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import cv2\n", + "import cv2.aruco as aruco\n", + "from typing import Sequence, cast\n", + "import awkward as ak\n", + "from pathlib import Path\n", + "import numpy as np\n", + "from typing import Final\n", + "from matplotlib import pyplot as plt\n", + "from cv2.typing import MatLike" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "A_PATH = Path(\"output/af_03.parquet\")\n", + "B_PATH = Path(\"output/ae_08.parquet\")\n", + "\n", + "a_params = ak.from_parquet(A_PATH)[0]\n", + "b_params = ak.from_parquet(B_PATH)[0]\n", + "display(a_params)\n", + "display(b_params)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "\n", + "\n", + "def create_new_aruco_marker_origin(marker_length: float):\n", + " \"\"\"\n", + " Create a new ArUco marker origin with the given length.\n", + "\n", + " 0 -> x\n", + " |\n", + " v\n", + " y\n", + "\n", + " 0---1\n", + " | |\n", + " 3---2\n", + "\n", + " So that the center of the marker is the origin for this PnP problem.\n", + "\n", + " Args:\n", + " marker_length: The length of the marker.\n", + " \"\"\"\n", + " return np.array(\n", + " [\n", + " [-marker_length / 2, marker_length / 2, 0],\n", + " [marker_length / 2, marker_length / 2, 0],\n", + " [marker_length / 2, -marker_length / 2, 0],\n", + " [-marker_length / 2, -marker_length / 2, 0],\n", + " ]\n", + " ).astype(np.float32)\n", + "\n", + "\n", + "DICTIONARY: Final[int] = aruco.DICT_4X4_50\n", + "# 400mm\n", + "MARKER_LENGTH: Final[float] = 0.4\n", + "aruco_dict = aruco.getPredefinedDictionary(DICTIONARY)\n", + "detector = aruco.ArucoDetector(\n", + " dictionary=aruco_dict, detectorParams=aruco.DetectorParameters()\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "a_img = cv2.imread(str(Path(\"dumped/marker/video-20241205-152716-board.png\")))\n", + "a_mtx = ak.to_numpy(a_params[\"camera_matrix\"])\n", + "a_dist = ak.to_numpy(a_params[\"distortion_coefficients\"])\n", + "\n", + "b_img = cv2.imread(str(Path(\"dumped/marker/video-20241205-152721-board.png\")))\n", + "b_mtx = ak.to_numpy(b_params[\"camera_matrix\"])\n", + "b_dist = ak.to_numpy(b_params[\"distortion_coefficients\"])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "a_corners, a_ids, _a_rejected = detector.detectMarkers(a_img)\n", + "b_corners, b_ids, _b_rejected = detector.detectMarkers(b_img)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ok, a_rvec, a_tvec = cv2.solvePnP(create_new_aruco_marker_origin(MARKER_LENGTH), a_corners[0], a_mtx, a_dist)\n", + "if not ok:\n", + " raise ValueError(\"Failed to solve PnP for A\")\n", + "a_img_output = cv2.drawFrameAxes(a_img, a_mtx, a_dist, a_rvec, a_tvec, MARKER_LENGTH)\n", + "plt.imshow(cv2.cvtColor(a_img_output, cv2.COLOR_BGR2RGB))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ok, b_rvec, b_tvec = cv2.solvePnP(create_new_aruco_marker_origin(MARKER_LENGTH), b_corners[0], b_mtx, b_dist)\n", + "if not ok:\n", + " raise ValueError(\"Failed to solve PnP for B\")\n", + "b_img_output = cv2.drawFrameAxes(b_img, b_mtx, b_dist, b_rvec, b_tvec, MARKER_LENGTH)\n", + "plt.imshow(cv2.cvtColor(b_img_output, cv2.COLOR_BGR2RGB))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Converts a rotation matrix to a rotation vector or vice versa\n", + "a_rmtx, _ = cv2.Rodrigues(a_rvec)\n", + "b_rmtx, _ = cv2.Rodrigues(b_rvec)\n", + "a_camera_coord = -(a_rmtx.T@ a_tvec)\n", + "b_camera_coord = -(b_rmtx.T @ b_tvec)\n", + "distance = np.linalg.norm(a_camera_coord - b_camera_coord)\n", + "a_distance = np.linalg.norm(a_camera_coord)\n", + "b_distance = np.linalg.norm(b_camera_coord)\n", + "display(\"d_ab={:.4}m a={:.4}m b={:.4}m\".format(distance, a_distance, b_distance))\n", + "display(\"a_coord={}\".format(a_camera_coord.T))\n", + "display(\"b_coord={}\".format(b_camera_coord.T))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def draw_grid(image: MatLike, rvec:MatLike, tvec:MatLike, camera_matrix:MatLike, dist_coeffs:MatLike, grid_size=10, grid_spacing=0.1):\n", + " # Create grid points in marker coordinate system\n", + " grid_points = []\n", + " for i in range(-grid_size, grid_size+1):\n", + " for j in range(-grid_size, grid_size+1):\n", + " grid_points.append([i*grid_spacing, j*grid_spacing, 0])\n", + " \n", + " grid_points = np.array(grid_points, dtype=np.float32)\n", + "\n", + " # Project grid points onto image plane\n", + " img_points, _ = cv2.projectPoints(grid_points, rvec, tvec, camera_matrix, dist_coeffs)\n", + " img_points = img_points.reshape(-1, 2)\n", + "\n", + " # Draw grid lines\n", + " for i in range(0, len(img_points), 2*grid_size+1):\n", + " cv2.polylines(image, [img_points[i:i+2*grid_size+1].astype(int)], False, (0, 255, 0), 1)\n", + " \n", + " for i in range(2*grid_size+1):\n", + " line_points = img_points[i::2*grid_size+1]\n", + " cv2.polylines(image, [line_points.astype(int)], False, (0, 255, 0), 1)\n", + "\n", + " return image" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "grid_image = draw_grid(a_img_output.copy(), a_rvec, a_tvec, a_mtx, a_dist, 10, 0.2)\n", + "plt.imshow(cv2.cvtColor(grid_image, cv2.COLOR_BGR2RGB))" + ] + } + ], + "metadata": { + "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.12.7" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}