From 2559055689d629b3304676df790d4ce1411bbd99 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Wed, 18 Dec 2024 18:04:50 +0800 Subject: [PATCH] PnP works, kinda --- boom.ipynb | 102 +++++++++++++++++++++++++++++++++++--- cali.py | 4 +- find_cute_object.py | 90 ++++++++++++++++++++++++++++++--- output/usbcam_cal.parquet | 4 +- 4 files changed, 182 insertions(+), 18 deletions(-) diff --git a/boom.ipynb b/boom.ipynb index 99bf65a..5af2b86 100644 --- a/boom.ipynb +++ b/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": { diff --git a/cali.py b/cali.py index b954e7d..31939ed 100644 --- a/cali.py +++ b/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" diff --git a/find_cute_object.py b/find_cute_object.py index c822f47..c86f70c 100644 --- a/find_cute_object.py +++ b/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( - size=(3, 3), squareLength=0.127, markerLength=0.097, dictionary=aruco_dict + detector = aruco.ArucoDetector( + 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( - grey - ) + markers, ids, rejected = detector.detectMarkers(grey) # `markers` is [N, 1, 4, 2] # `ids` is [N, 1] - if diamond_ids is not None: - aruco.drawDetectedDiamonds(frame, diamond_corners, diamond_ids) + if ids is not None: + 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") diff --git a/output/usbcam_cal.parquet b/output/usbcam_cal.parquet index 4e61bbd..8d95d89 100644 --- a/output/usbcam_cal.parquet +++ b/output/usbcam_cal.parquet @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:64da4ba931f6e118e0cf74b75468ec345c43bab5bf7a61ab88e11ccdb3559a79 -size 54606 +oid sha256:22cdc0b63c19e85e94d6ad17d19a39c7f5f85fef44eb29e978d6e63160a810fa +size 74567