more paramters

.gitignore

@@ -166,3 +166,4 @@ cython_debug/
 #.idea/
 .DS_Store
 output/svg
+*.mp4

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": [

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):

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__":

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": [
+       "<pre>[{name: &#x27;a&#x27;, rvec: [[-1.04], ..., [-2.95]], tvec: [...], ...},\n",
+       " {name: &#x27;b&#x27;, 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",
+       "}</pre>"
+      ],
+      "text/plain": [
+       "<Array [{name: 'a', rvec: [...], ...}, ...] type='2 * {name: string, rvec: ...'>"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    },
+    {
+     "data": {
+      "text/plain": [
+       "<pyarrow._parquet.FileMetaData object at 0x17f93f830>\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
+}