diff --git a/.gitignore b/.gitignore index 059c47a..b475da3 100644 --- a/.gitignore +++ b/.gitignore @@ -166,3 +166,4 @@ cython_debug/ #.idea/ .DS_Store output/svg +*.mp4 \ No newline at end of file diff --git a/boom.ipynb b/boom.ipynb index 5af2b86..afe08d0 100644 --- a/boom.ipynb +++ b/boom.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 38, + "execution_count": 95, "metadata": {}, "outputs": [], "source": [ @@ -118,7 +118,7 @@ }, { "cell_type": "code", - "execution_count": 39, + "execution_count": 96, "metadata": {}, "outputs": [ { @@ -155,7 +155,7 @@ }, { "cell_type": "code", - "execution_count": 40, + "execution_count": 97, "metadata": {}, "outputs": [], "source": [ @@ -284,12 +284,13 @@ }, { "cell_type": "code", - "execution_count": 41, + "execution_count": 98, "metadata": {}, "outputs": [], "source": [ "plane_a = DiamondPlane3D(markers)\n", "\n", + "OFFSET = 0.000\n", "markers_b = generate_diamond_corners((20, 21, 22, 23), params)\n", "plane_b = DiamondPlane3D(markers_b)\n", "# plane_b.translate(np.array([0, 0, 0.1]))\n", @@ -300,6 +301,9 @@ "plane_b.rotate(np.pi, np.array([0, 1, 0]))\n", "plane_b.translate(tmp_c)\n", "plane_b.translate(np.array([0, 0, params.total_side_length]))\n", + "plane_b.translate(np.array([0, 0, -OFFSET]))\n", + "# OFFSET for plane_b\n", + "# plane_b.translate(np.array([0, 0.001, 0]))\n", "\n", "markers_c = generate_diamond_corners((24, 25, 26, 27), params)\n", "plane_c = DiamondPlane3D(markers_c)\n", @@ -309,12 +313,13 @@ "plane_c.translate(tmp)\n", "plane_c.translate(np.array([0, params.total_side_length-params.border_length, 0]))\n", "plane_c.rotate(np.pi/2, np.array([0, 1, 0]))\n", - "plane_c.translate(np.array([0, 0, params.total_side_length]))" + "plane_c.translate(np.array([0, 0, params.total_side_length]))\n", + "plane_c.translate(np.array([0, 0, -OFFSET]))" ] }, { "cell_type": "code", - "execution_count": 42, + "execution_count": 99, "metadata": {}, "outputs": [ { @@ -1722,7 +1727,7 @@ }, { "cell_type": "code", - "execution_count": 43, + "execution_count": 100, "metadata": {}, "outputs": [ { @@ -1775,7 +1780,7 @@ }, { "cell_type": "code", - "execution_count": 44, + "execution_count": 101, "metadata": {}, "outputs": [], "source": [ @@ -1787,7 +1792,7 @@ }, { "cell_type": "code", - "execution_count": 45, + "execution_count": 102, "metadata": {}, "outputs": [ { @@ -1843,7 +1848,7 @@ " [2.30233590e-17, 2.49000005e-01, 3.50000129e-02]])}" ] }, - "execution_count": 45, + "execution_count": 102, "metadata": {}, "output_type": "execute_result" } @@ -1854,7 +1859,7 @@ }, { "cell_type": "code", - "execution_count": 46, + "execution_count": 103, "metadata": {}, "outputs": [], "source": [ diff --git a/cali.py b/cali.py index 31939ed..f067b4c 100644 --- a/cali.py +++ b/cali.py @@ -37,10 +37,10 @@ class ArucoDictionary(Enum): Dict_ArUco_ORIGINAL = aruco.DICT_ARUCO_ORIGINAL -IMAGE_FOLDER = Path("dumped/cam") +IMAGE_FOLDER = Path("dumped/batch_two/b") OUTPUT_FOLDER = Path("output") DICTIONARY = ArucoDictionary.Dict_4X4_50 -CALIBRATION_PARQUET: Optional[Path] = OUTPUT_FOLDER / "usbcam_cal.parquet" +CALIBRATION_PARQUET: Optional[Path] = OUTPUT_FOLDER / "af_03.parquet" class CameraParams(TypedDict): diff --git a/find_cute_object.py b/find_cute_object.py index c86f70c..9c15990 100644 --- a/find_cute_object.py +++ b/find_cute_object.py @@ -1,10 +1,9 @@ -import re import cv2 from cv2 import aruco from datetime import datetime from loguru import logger from pathlib import Path -from typing import cast, Final +from typing import Optional, cast, Final import awkward as ak from cv2.typing import MatLike import numpy as np @@ -42,6 +41,7 @@ def main(): total_corners = cast(NDArray, ak.to_numpy(ops["corners"])).reshape(-1, 4, 3) ops_map: dict[int, NDArray] = dict(zip(total_ids, total_corners)) logger.info("ops_map={}", ops_map) + writer: Optional[cv2.VideoWriter] = None for frame in gen(): grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) @@ -123,6 +123,8 @@ def main(): else: logger.warning("Failed to solvePnPRansac") cv2.imshow("frame", frame) + if writer is not None: + writer.write(frame) if (k := cv2.waitKey(1)) == ord("q"): logger.info("Exiting") break @@ -131,6 +133,17 @@ def main(): file_name = f"aruco_{now}.png" logger.info("Saving to {}", file_name) cv2.imwrite(file_name, frame) + elif k == ord("r"): + if writer is not None: + writer.release() + writer = None + logger.info("Recording stopped") + else: + now = datetime.now().strftime("%Y%m%d%H%M%S") + file_name = f"aruco_{now}.mp4" + logger.info("Recording to {}", file_name) + fourcc = cv2.VideoWriter.fourcc(*"mp4v") + writer = cv2.VideoWriter(file_name, fourcc, 20.0, frame.shape[:2][::-1]) if __name__ == "__main__": diff --git a/get_ext.ipynb b/get_ext.ipynb new file mode 100644 index 0000000..50f68b0 --- /dev/null +++ b/get_ext.ipynb @@ -0,0 +1,396 @@ +{ + "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": [
+ "