213 lines
6.5 KiB
Plaintext
213 lines
6.5 KiB
Plaintext
{
|
|
"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
|
|
}
|