HQU-gxy/ChArUcoBoardExp
7
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PnP works, kinda

Browse Source
This commit is contained in:
crosstyan
2024-12-18 18:04:50 +08:00
parent a207c90cb9
commit 2559055689
4 changed files with 182 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
boom.ipynb
View File

@ -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": {

4
cali.py
View File

@ -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"

90
find_cute_object.py
View File

@ -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")

BIN
output/usbcam_cal.parquet LFS
View File

Binary file not shown.