PnP works, kinda

boom.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": 38,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -118,7 +118,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 39,
    "metadata": {},
    "outputs": [
     {
@@ -155,7 +155,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 40,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -284,7 +284,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 41,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -314,7 +314,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 15,
+   "execution_count": 42,
    "metadata": {},
    "outputs": [
     {
@@ -1722,7 +1722,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 16,
+   "execution_count": 43,
    "metadata": {},
    "outputs": [
     {
@@ -1772,6 +1772,96 @@
     "display(coords)\n",
     "_ = ak.to_parquet(coords, \"output/object_points.parquet\")"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 44,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from typing import cast\n",
+    "total_ids = cast(NDArray, ak.to_numpy(coords[\"ids\"])).flatten()\n",
+    "total_corners = cast(NDArray, ak.to_numpy(coords[\"corners\"])).reshape(-1, 4, 3)\n",
+    "#display(total_ids, total_corners)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 45,
+   "metadata": {},
+   "outputs": [
+    {
+     "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]])}"
+      ]
+     },
+     "execution_count": 45,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "dict(zip(total_ids, total_corners))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 46,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "total_ids = np.concatenate([plane_a.ids, plane_b.ids, plane_c.ids])\n",
+    "total_corners = np.concatenate([t_corners_a, t_corners_b, t_corners_c])\n",
+    "id_corner_map: dict[int, NDArray] = dict(zip(total_ids, total_corners))"
+   ]
   }
  ],
  "metadata": {

cali.py

@@ -37,7 +37,7 @@ class ArucoDictionary(Enum):
     Dict_ArUco_ORIGINAL = aruco.DICT_ARUCO_ORIGINAL
 
 
-IMAGE_FOLDER = Path("dumped/usbcam")
+IMAGE_FOLDER = Path("dumped/cam")
 OUTPUT_FOLDER = Path("output")
 DICTIONARY = ArucoDictionary.Dict_4X4_50
 CALIBRATION_PARQUET: Optional[Path] = OUTPUT_FOLDER / "usbcam_cal.parquet"
@@ -140,7 +140,7 @@ def main():
             "rotation_vectors": rvecs,
             "translation_vectors": tvecs,
         }
-        ak.to_parquet([parameters], OUTPUT_FOLDER / "calibration.parquet")
+        ak.to_parquet([parameters], CALIBRATION_PARQUET)
     else:
         logger.warning(
             "no calibration data calculated; either no images or already calibrated"

find_cute_object.py

@@ -1,3 +1,4 @@
+import re
 import cv2
 from cv2 import aruco
 from datetime import datetime
@@ -33,21 +34,94 @@ def main():
     camera_matrix = cast(MatLike, ak.to_numpy(cal["camera_matrix"]))
     distortion_coefficients = cast(MatLike, ak.to_numpy(cal["distortion_coefficients"]))
     ops = ak.from_parquet(OBJECT_POINTS_PARQUET)
-    board = aruco.CharucoBoard(
+    detector = aruco.ArucoDetector(
-        size=(3, 3), squareLength=0.127, markerLength=0.097, dictionary=aruco_dict
+        dictionary=aruco_dict, detectorParams=aruco.DetectorParameters()
     )
-    detector = aruco.CharucoDetector(board)
+
+    total_ids = cast(NDArray, ak.to_numpy(ops["ids"])).flatten()
+    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)
 
     for frame in gen():
         grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
         # pylint: disable-next=unpacking-non-sequence
-        diamond_corners, diamond_ids, markers, marker_ids = detector.detectDiamonds(
+        markers, ids, rejected = detector.detectMarkers(grey)
-            grey
-        )
         # `markers` is [N, 1, 4, 2]
         # `ids` is [N, 1]
-        if diamond_ids is not None:
+        if ids is not None:
-            aruco.drawDetectedDiamonds(frame, diamond_corners, diamond_ids)
+            markers = np.reshape(markers, (-1, 4, 2))
+            ids = np.reshape(ids, (-1, 1))
+            # logger.info("markers={}, ids={}", np.array(markers).shape, np.array(ids).shape)
+            ips_map: dict[int, NDArray] = {}
+            for cs, id in zip(markers, ids):
+                id = int(id)
+                cs = cast(NDArray, cs)
+                ips_map[id] = cs
+                center = np.mean(cs, axis=0).astype(int)
+                GREY = (128, 128, 128)
+                # logger.info("id={}, center={}", id, center)
+                cv2.circle(frame, tuple(center), 5, GREY, -1)
+                cv2.putText(
+                    frame,
+                    str(id),
+                    tuple(center),
+                    cv2.FONT_HERSHEY_SIMPLEX,
+                    1,
+                    GREY,
+                    2,
+                )
+                # BGR
+                RED = (0, 0, 255)
+                GREEN = (0, 255, 0)
+                BLUE = (255, 0, 0)
+                YELLOW = (0, 255, 255)
+                color_map = [RED, GREEN, BLUE, YELLOW]
+                for color, corners in zip(color_map, cs):
+                    corners = corners.astype(int)
+                    frame = cv2.circle(frame, corners, 5, color, -1)
+            # https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e
+            ops: NDArray = np.empty((0, 3), dtype=np.float32)
+            ips: NDArray = np.empty((0, 2), dtype=np.float32)
+            for id, ip in ips_map.items():
+                try:
+                    op = ops_map[id]
+                    assert ip.shape == (4, 2), f"corners.shape={ip.shape}"
+                    assert op.shape == (4, 3), f"op.shape={op.shape}"
+                    ops = np.concatenate((ops, op), axis=0)
+                    ips = np.concatenate((ips, ip), axis=0)
+                except KeyError:
+                    logger.warning("No object points for id={}", id)
+                    continue
+            assert len(ops) == len(ips), f"len(ops)={len(ops)} != len(ips)={len(ips)}"
+            if len(ops) > 0:
+                # https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html
+                # https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html#calib3d_solvePnP_flags
+                ret, rvec, tvec= cv2.solvePnP(
+                    objectPoints=ops,
+                    imagePoints=ips,
+                    cameraMatrix=camera_matrix,
+                    distCoeffs=distortion_coefficients,
+                    flags=cv2.SOLVEPNP_SQPNP,
+                )
+                # ret, rvec, tvec, inliners = cv2.solvePnPRansac(
+                #     objectPoints=ops,
+                #     imagePoints=ips,
+                #     cameraMatrix=camera_matrix,
+                #     distCoeffs=distortion_coefficients,
+                #     flags=cv2.SOLVEPNP_SQPNP,
+                # )
+                if ret:
+                    cv2.drawFrameAxes(
+                        frame,
+                        camera_matrix,
+                        distortion_coefficients,
+                        rvec,
+                        tvec,
+                        MARKER_LENGTH,
+                    )
+                else:
+                    logger.warning("Failed to solvePnPRansac")
         cv2.imshow("frame", frame)
         if (k := cv2.waitKey(1)) == ord("q"):
             logger.info("Exiting")