{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import cv2\n",
"from cv2 import aruco\n",
"from datetime import datetime\n",
"from loguru import logger\n",
"from pathlib import Path\n",
"from typing import Optional, cast, Final\n",
"import awkward as ak\n",
"from cv2.typing import MatLike\n",
"import numpy as np\n",
"from matplotlib import pyplot as plt\n",
"import awkward as ak\n",
"from awkward import Record as AwkwardRecord, Array as AwkwardArray"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"NDArray = np.ndarray\n",
"OBJECT_POINTS_PARQUET = Path(\"output\") / \"object_points.parquet\"\n",
"DICTIONARY: Final[int] = aruco.DICT_4X4_50\n",
"# 400mm\n",
"MARKER_LENGTH: Final[float] = 0.4\n",
"\n",
"A_CALIBRATION_PARQUET = Path(\"output\") / \"a-ae_08.parquet\"\n",
"B_CALIBRATION_PARQUET = Path(\"output\") / \"b-af_03.parquet\""
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"'ops_map'"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/plain": [
"{16: array([[0.152, 0.025, 0. ],\n",
" [0.249, 0.025, 0. ],\n",
" [0.249, 0.122, 0. ],\n",
" [0.152, 0.122, 0. ]]),\n",
" 17: array([[0.025, 0.152, 0. ],\n",
" [0.122, 0.152, 0. ],\n",
" [0.122, 0.249, 0. ],\n",
" [0.025, 0.249, 0. ]]),\n",
" 18: array([[0.27900001, 0.152 , 0. ],\n",
" [0.37599999, 0.152 , 0. ],\n",
" [0.37599999, 0.249 , 0. ],\n",
" [0.27900001, 0.249 , 0. ]]),\n",
" 19: array([[0.152 , 0.27900001, 0. ],\n",
" [0.249 , 0.27900001, 0. ],\n",
" [0.249 , 0.37599999, 0. ],\n",
" [0.152 , 0.37599999, 0. ]]),\n",
" 20: array([[1.51999995e-01, 1.70838222e-17, 3.86000000e-01],\n",
" [2.48999998e-01, 2.89628965e-17, 3.86000000e-01],\n",
" [2.48999998e-01, 2.30233595e-17, 2.88999999e-01],\n",
" [1.51999995e-01, 1.11442852e-17, 2.88999999e-01]]),\n",
" 21: array([[ 2.50000004e-02, -6.24569833e-18, 2.59000005e-01],\n",
" [ 1.22000001e-01, 5.63337574e-18, 2.59000005e-01],\n",
" [ 1.22000001e-01, -3.06161408e-19, 1.62000002e-01],\n",
" [ 2.50000004e-02, -1.21852355e-17, 1.62000002e-01]]),\n",
" 22: array([[2.79000014e-01, 2.48603320e-17, 2.59000005e-01],\n",
" [3.75999987e-01, 3.67394027e-17, 2.59000005e-01],\n",
" [3.75999987e-01, 3.07998655e-17, 1.62000002e-01],\n",
" [2.79000014e-01, 1.89207949e-17, 1.62000002e-01]]),\n",
" 23: array([[ 1.51999995e-01, 1.53080704e-18, 1.31999986e-01],\n",
" [ 2.48999998e-01, 1.34098813e-17, 1.31999986e-01],\n",
" [ 2.48999998e-01, 7.47034601e-18, 3.50000129e-02],\n",
" [ 1.51999995e-01, -4.40872829e-18, 3.50000129e-02]]),\n",
" 24: array([[1.53080852e-18, 2.49000005e-01, 3.86000000e-01],\n",
" [1.53080852e-18, 1.52000002e-01, 3.86000000e-01],\n",
" [7.47034556e-18, 1.52000002e-01, 2.88999999e-01],\n",
" [7.47034556e-18, 2.49000005e-01, 2.88999999e-01]]),\n",
" 25: array([[9.30731537e-18, 3.76000000e-01, 2.59000005e-01],\n",
" [9.30731537e-18, 2.78999999e-01, 2.59000005e-01],\n",
" [1.52468525e-17, 2.78999999e-01, 1.62000002e-01],\n",
" [1.52468525e-17, 3.76000000e-01, 1.62000002e-01]]),\n",
" 26: array([[9.30731537e-18, 1.21999986e-01, 2.59000005e-01],\n",
" [9.30731537e-18, 2.50000129e-02, 2.59000005e-01],\n",
" [1.52468525e-17, 2.50000129e-02, 1.62000002e-01],\n",
" [1.52468525e-17, 1.21999986e-01, 1.62000002e-01]]),\n",
" 27: array([[1.70838237e-17, 2.49000005e-01, 1.31999986e-01],\n",
" [1.70838237e-17, 1.52000002e-01, 1.31999986e-01],\n",
" [2.30233590e-17, 1.52000002e-01, 3.50000129e-02],\n",
" [2.30233590e-17, 2.49000005e-01, 3.50000129e-02]])}"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"aruco_dict = aruco.getPredefinedDictionary(DICTIONARY)\n",
"def read_camera_calibration(path: Path) -> tuple[MatLike, MatLike]:\n",
" cal = ak.from_parquet(path)[0]\n",
" camera_matrix = cast(MatLike, ak.to_numpy(cal[\"camera_matrix\"]))\n",
" distortion_coefficients = cast(MatLike, ak.to_numpy(cal[\"distortion_coefficients\"]))\n",
" return camera_matrix, distortion_coefficients\n",
"\n",
"a_mtx, a_dist = read_camera_calibration(A_CALIBRATION_PARQUET)\n",
"b_camera_matrix, b_distortion_coefficients = read_camera_calibration(B_CALIBRATION_PARQUET)\n",
"ops = ak.from_parquet(OBJECT_POINTS_PARQUET)\n",
"detector = aruco.ArucoDetector(\n",
" dictionary=aruco_dict, detectorParams=aruco.DetectorParameters()\n",
")\n",
"\n",
"total_ids = cast(NDArray, ak.to_numpy(ops[\"ids\"])).flatten()\n",
"total_corners = cast(NDArray, ak.to_numpy(ops[\"corners\"])).reshape(-1, 4, 3)\n",
"ops_map: dict[int, NDArray] = dict(zip(total_ids, total_corners))\n",
"display(\"ops_map\", ops_map)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"def process(\n",
" frame: MatLike,\n",
" cam_mtx: MatLike,\n",
" dist_coeffs: MatLike,\n",
" target: Optional[MatLike] = None,\n",
") -> tuple[MatLike, Optional[MatLike], Optional[MatLike]]:\n",
" if target is None:\n",
" target = frame.copy()\n",
" grey = cv2.cvtColor(target, cv2.COLOR_BGR2GRAY)\n",
" # pylint: disable-next=unpacking-non-sequence\n",
" markers, ids, rejected = detector.detectMarkers(grey)\n",
" # `markers` is [N, 1, 4, 2]\n",
" # `ids` is [N, 1]\n",
" ret_rvec: Optional[MatLike] = None\n",
" ret_tvec: Optional[MatLike] = None\n",
" if ids is not None:\n",
" markers = np.reshape(markers, (-1, 4, 2))\n",
" ids = np.reshape(ids, (-1, 1))\n",
" # logger.info(\"markers={}, ids={}\", np.array(markers).shape, np.array(ids).shape)\n",
" ips_map: dict[int, NDArray] = {}\n",
" for cs, id in zip(markers, ids):\n",
" id = int(id)\n",
" cs = cast(NDArray, cs)\n",
" ips_map[id] = cs\n",
" center = np.mean(cs, axis=0).astype(int)\n",
" GREY = (128, 128, 128)\n",
" # logger.info(\"id={}, center={}\", id, center)\n",
" cv2.circle(target, tuple(center), 5, GREY, -1)\n",
" cv2.putText(\n",
" target,\n",
" str(id),\n",
" tuple(center),\n",
" cv2.FONT_HERSHEY_SIMPLEX,\n",
" 1,\n",
" GREY,\n",
" 2,\n",
" )\n",
" # BGR\n",
" RED = (0, 0, 255)\n",
" GREEN = (0, 255, 0)\n",
" BLUE = (255, 0, 0)\n",
" YELLOW = (0, 255, 255)\n",
" color_map = [RED, GREEN, BLUE, YELLOW]\n",
" for color, corners in zip(color_map, cs):\n",
" corners = corners.astype(int)\n",
" target = cv2.circle(target, corners, 5, color, -1)\n",
" # https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e\n",
" ops: NDArray = np.empty((0, 3), dtype=np.float32)\n",
" ips: NDArray = np.empty((0, 2), dtype=np.float32)\n",
" for id, ip in ips_map.items():\n",
" try:\n",
" op = ops_map[id]\n",
" assert ip.shape == (4, 2), f\"corners.shape={ip.shape}\"\n",
" assert op.shape == (4, 3), f\"op.shape={op.shape}\"\n",
" ops = np.concatenate((ops, op), axis=0)\n",
" ips = np.concatenate((ips, ip), axis=0)\n",
" except KeyError:\n",
" logger.warning(\"No object points for id={}\", id)\n",
" continue\n",
" assert len(ops) == len(ips), f\"len(ops)={len(ops)} != len(ips)={len(ips)}\"\n",
" if len(ops) > 0:\n",
" # https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html\n",
" # https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html#calib3d_solvePnP_flags\n",
" ret, rvec, tvec = cv2.solvePnP(\n",
" objectPoints=ops,\n",
" imagePoints=ips,\n",
" cameraMatrix=cam_mtx,\n",
" distCoeffs=dist_coeffs,\n",
" flags=cv2.SOLVEPNP_SQPNP,\n",
" )\n",
" # ret, rvec, tvec, inliners = cv2.solvePnPRansac(\n",
" # objectPoints=ops,\n",
" # imagePoints=ips,\n",
" # cameraMatrix=camera_matrix,\n",
" # distCoeffs=distortion_coefficients,\n",
" # flags=cv2.SOLVEPNP_SQPNP,\n",
" # )\n",
" if ret:\n",
" cv2.drawFrameAxes(\n",
" target,\n",
" cam_mtx,\n",
" dist_coeffs,\n",
" rvec,\n",
" tvec,\n",
" MARKER_LENGTH,\n",
" )\n",
" ret_rvec = rvec\n",
" ret_tvec = tvec\n",
" return target, ret_rvec, ret_tvec"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"A_IMG = Path(\"dumped/batch_two/op/video-20241219-142434-op-a.png\")\n",
"B_IMG = Path(\"dumped/batch_two/op/video-20241219-142439-op-b.png\")\n",
"a_img = cv2.imread(str(A_IMG))\n",
"b_img = cv2.imread(str(B_IMG))"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/var/folders/cj/0zmvpygn7m72m42lh6x_hcgw0000gn/T/ipykernel_28395/542219436.py:22: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" id = int(id)\n"
]
}
],
"source": [
"a_result_img, a_rvec, a_tvec = process(a_img, a_mtx, a_dist)\n",
"# plt.imshow(cv2.cvtColor(a_result_img, cv2.COLOR_BGR2RGB))"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/var/folders/cj/0zmvpygn7m72m42lh6x_hcgw0000gn/T/ipykernel_28395/542219436.py:22: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" id = int(id)\n"
]
}
],
"source": [
"b_result_img, b_rvec, b_tvec = process(b_img, b_camera_matrix, b_distortion_coefficients)\n",
"# plt.imshow(cv2.cvtColor(b_result_img, cv2.COLOR_BGR2RGB))"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"'params'"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/html": [
"[{name: 'a', rvec: [[-1.04], ..., [-2.95]], tvec: [...], ...},\n",
" {name: 'b', rvec: [[0.948], ..., [2.97]], tvec: [...], ...}]\n",
"--------------------------------------------------------------\n",
"type: 2 * {\n",
" name: string,\n",
" rvec: var * var * float64,\n",
" tvec: var * var * float64,\n",
" camera_matrix: var * var * float64,\n",
" distortion_coefficients: var * var * float64\n",
"}"
],
"text/plain": [
""
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/plain": [
"\n",
" created_by: parquet-cpp-arrow version 14.0.1\n",
" num_columns: 5\n",
" num_rows: 2\n",
" num_row_groups: 1\n",
" format_version: 2.6\n",
" serialized_size: 0"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"params = AwkwardArray(\n",
" [\n",
" {\n",
" \"name\": \"a\",\n",
" \"rvec\": a_rvec,\n",
" \"tvec\": a_tvec,\n",
" \"camera_matrix\": a_mtx,\n",
" \"distortion_coefficients\": a_dist,\n",
" },\n",
" {\n",
" \"name\": \"b\",\n",
" \"rvec\": b_rvec,\n",
" \"tvec\": b_tvec,\n",
" \"camera_matrix\": b_camera_matrix,\n",
" \"distortion_coefficients\": b_distortion_coefficients,\n",
" },\n",
" ]\n",
")\n",
"display(\"params\", params)\n",
"ak.to_parquet(params, Path(\"output\") / \"params.parquet\")"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"True"
]
},
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"cv2.imwrite(\"output/a_result_img.png\", a_result_img)\n",
"cv2.imwrite(\"output/b_result_img.png\", b_result_img)"
]
}
],
"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.8"
}
},
"nbformat": 4,
"nbformat_minor": 2
}