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

Compare commits

base: HQU-gxy:733c6f8670889c2c53308cf241c9cce946c500be
Branches Tags
HQU-gxy:main
...
compare: HQU-gxy:main
Branches Tags
HQU-gxy:main

2 Commits
733c6f8670 ... main

Author SHA1 Message Date
crosstyan
e593c7b363 Refactor find_extrinsic_object.py: update object points parquet file name, enhance type annotations with MarkerFace TypedDict for diamond ArUco markers, and improve code organization. 2025-04-30 11:45:41 +08:00
crosstyan
c8f4a7ab26 Refactor Jupyter notebooks for marker processing: deleted boom.ipynb and compute_3d_maybe.ipynb, added calculate_box_coord_naive.ipynb, calculate_box_face_coord_naive.ipynb, estimate_extrinstic.ipynb, find_aruco_points_with_image.ipynb, find_aruco_points.py, and find_extrinsic_object.py. Updated .gitignore to include new output files. 2025-04-30 11:43:59 +08:00
15 changed files with 875 additions and 2593 deletions
Expand all files Collapse all files

5
.gitignore vendored
View File

@ -165,6 +165,7 @@ cython_debug/
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
#.idea/
.DS_Store
output/svg
*.mp4
output
output/*.json
~output/standard_box_markers.parquet
~output/object_points.parquet

1893
boom.ipynb
View File

File diff suppressed because one or more lines are too long

497
calculate_box_coord_naive.ipynb Normal file
View File

@ -0,0 +1,497 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from dataclasses import dataclass\n",
"import numpy as np\n",
"from matplotlib import pyplot as plt\n",
"\n",
"NDArray = np.ndarray\n",
"\n",
"# Order of detection result\n",
"# 0, 1, 2, 3\n",
"# TL, TR, BR, BL\n",
"# RED, GREEN, BLUE, YELLOW\n",
"\n",
"\n",
"@dataclass\n",
"class DiamondBoardParameter:\n",
" marker_leghth: float\n",
" \"\"\"\n",
" the ArUco marker length in meter\n",
" \"\"\"\n",
" chess_length: float\n",
" \"\"\"\n",
" the length of the chess board in meter\n",
" \"\"\"\n",
" border_length: float = 0.01\n",
" \"\"\"\n",
" border_length in m, default is 1cm\n",
" \"\"\"\n",
"\n",
" @property\n",
" def marker_border_length(self):\n",
" assert self.chess_length > self.marker_leghth\n",
" return (self.chess_length - self.marker_leghth) / 2\n",
"\n",
" @property\n",
" def total_side_length(self):\n",
" assert self.chess_length > self.marker_leghth\n",
" return self.marker_border_length * 2 + self.chess_length * 3\n",
"\n",
"\n",
"# 9mm + 127mm + 127mm (97mm marker) + 127mm + 10mm\n",
"# i.e. marker boarder = 127mm - 97mm = 30mm (15mm each side)\n",
"Point2D = tuple[float, float]\n",
"Quad2D = tuple[Point2D, Point2D, Point2D, Point2D]\n",
"\n",
"\n",
"@dataclass\n",
"class ArUcoMarker2D:\n",
" id: int\n",
" corners: Quad2D\n",
" params: DiamondBoardParameter\n",
"\n",
" @property\n",
" def np_corners(self):\n",
" \"\"\"\n",
" returns corners in numpy array\n",
" (4, 2) shape\n",
" \"\"\"\n",
" return np.array(self.corners, dtype=np.float32)\n",
"\n",
"\n",
"# let's let TL be the origin\n",
"def generate_diamond_corners(\n",
" ids: tuple[int, int, int, int], params: DiamondBoardParameter\n",
"):\n",
" \"\"\"\n",
" A diamond chess board, which could be count as a kind of ChArUco board\n",
"\n",
" C | 0 | C\n",
" ---------\n",
" 1 | C | 2\n",
" ---------\n",
" C | 3 | C\n",
"\n",
" where C is the chess box, and 0, 1, 2, 3 are the markers (whose ids are passed in order)\n",
"\n",
" Args:\n",
" ids: a tuple of 4 ids of the markers\n",
" params: DiamondBoardParameter\n",
" \"\"\"\n",
"\n",
" def tl_to_square(tl_x: float, tl_y: float, side_length: float) -> Quad2D:\n",
" return (\n",
" (tl_x, tl_y),\n",
" (tl_x + side_length, tl_y),\n",
" (tl_x + side_length, tl_y + side_length),\n",
" (tl_x, tl_y + side_length),\n",
" )\n",
"\n",
" tl_0_x = params.border_length + params.chess_length + params.marker_border_length\n",
" tl_0_y = params.border_length + params.marker_border_length\n",
"\n",
" tl_1_x = params.border_length + params.marker_border_length\n",
" tl_1_y = params.border_length + params.chess_length + params.marker_border_length\n",
"\n",
" tl_2_x = (\n",
" params.border_length + params.chess_length * 2 + params.marker_border_length\n",
" )\n",
" tl_2_y = tl_1_y\n",
"\n",
" tl_3_x = params.border_length + params.chess_length + params.marker_border_length\n",
" tl_3_y = (\n",
" params.border_length + params.chess_length * 2 + params.marker_border_length\n",
" )\n",
" return (\n",
" ArUcoMarker2D(ids[0], tl_to_square(tl_0_x, tl_0_y, params.marker_leghth), params),\n",
" ArUcoMarker2D(ids[1], tl_to_square(tl_1_x, tl_1_y, params.marker_leghth), params),\n",
" ArUcoMarker2D(ids[2], tl_to_square(tl_2_x, tl_2_y, params.marker_leghth), params),\n",
" ArUcoMarker2D(ids[3], tl_to_square(tl_3_x, tl_3_y, params.marker_leghth), params),\n",
" )\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"params = DiamondBoardParameter(0.097, 0.127)\n",
"markers = generate_diamond_corners((16, 17, 18, 19), params)\n",
"\n",
"fig = plt.figure()\n",
"ax = fig.gca()\n",
"ax.set_xlim((0, params.total_side_length))\n",
"ax.set_ylim((0, params.total_side_length)) # type: ignore\n",
"ax.set_aspect(\"equal\")\n",
"# set origin to top-left (from bottom-left)\n",
"ax.invert_yaxis()\n",
"ax.xaxis.set_ticks_position('top')\n",
"\n",
"for marker in markers:\n",
" plt.plot(*marker.np_corners.T, \"o-\", label=str(marker.id))\n",
" for i, (x, y) in enumerate(marker.corners):\n",
" ax.text(x, y, str(i))\n",
"plt.legend()\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"from typing import Sequence\n",
"import plotly.graph_objects as go\n",
"import awkward as ak\n",
"import cv2\n",
"from cv2.typing import MatLike\n",
"\n",
"\n",
"def transform_point(matrix: MatLike, point: MatLike):\n",
" assert matrix.shape == (4, 4)\n",
" assert point.shape == (3,)\n",
"\n",
" # Lift point to 4D\n",
" homogeneous_point = np.array([point[0], point[1], point[2], 1])\n",
" # Apply transformation\n",
" transformed = matrix @ homogeneous_point\n",
" # Project back to 3D if w != 1\n",
" if transformed[3] != 1:\n",
" transformed = transformed / transformed[3]\n",
" return transformed[:3]\n",
"\n",
"\n",
"class DiamondPlane3D:\n",
" _ids: NDArray\n",
" \"\"\"\n",
" (n,)\n",
" \"\"\"\n",
" _corners: NDArray\n",
" \"\"\"\n",
" (n, 4, 3)\n",
" \"\"\"\n",
" _transform_matrix: NDArray\n",
" \"\"\"\n",
" 4x4 transformation matrix\n",
" \"\"\"\n",
" _normal_vector: NDArray\n",
" \"\"\"\n",
" (2, 3)\n",
" start (the center of the plane) and end (the normal vector), length 1\n",
" \"\"\"\n",
"\n",
" def __init__(self, items: Sequence[ArUcoMarker2D]):\n",
" self._ids = np.array([item.id for item in items])\n",
" # (n, 4, 2)\n",
" corners_2d = np.array([item.np_corners for item in items])\n",
" # (n, 4, 3)\n",
" self._corners = np.concatenate(\n",
" [corners_2d, np.zeros((corners_2d.shape[0], 4, 1))], axis=-1\n",
" )\n",
" self._transform_matrix = np.eye(4)\n",
"\n",
" def center(items: Sequence[ArUcoMarker2D]):\n",
" return np.mean([item.np_corners for item in items], axis=(0, 1))\n",
"\n",
" c = center(items)\n",
" assert c.shape == (2,)\n",
" self._normal_vector = np.array([(c[0], c[1], 0), (c[0], c[1], 0.1)])\n",
"\n",
" @property\n",
" def ids(self):\n",
" return self._ids\n",
"\n",
" @property\n",
" def corners(self):\n",
" return self._corners\n",
"\n",
" @property\n",
" def transform_matrix(self):\n",
" return self._transform_matrix\n",
"\n",
" @property\n",
" def transformed_corners(self):\n",
" def g():\n",
" for corner in self.corners:\n",
" yield np.array(\n",
" [transform_point(self.transform_matrix, c) for c in corner]\n",
" )\n",
"\n",
" return np.array(list(g()))\n",
"\n",
" @property\n",
" def transformed_normal_vector(self):\n",
" def g():\n",
" for v in self._normal_vector:\n",
" yield transform_point(self.transform_matrix, v)\n",
"\n",
" return np.array(list(g()))\n",
"\n",
" @property\n",
" def transformed_geometry_center(self):\n",
" return np.mean(self.transformed_corners, axis=(0, 1))\n",
"\n",
" def local_rotate(self, angle: float, axis: NDArray):\n",
" \"\"\"\n",
" rotate the plane by angle (in radian) around local center\n",
"\n",
" Args:\n",
" angle: in radian\n",
" axis: (3,)\n",
"\n",
" change basis to local basis, rotate, then change back\n",
" \"\"\"\n",
" raise NotImplementedError\n",
"\n",
" def rotate(self, angle: float, axis: NDArray):\n",
" \"\"\"\n",
" rotate the plane by angle (in radian) around the axis\n",
" \"\"\"\n",
" assert axis.shape == (3,)\n",
" rot_mat = cv2.Rodrigues(axis * angle)[0]\n",
" self._transform_matrix[:3, :3] = np.dot(rot_mat, self._transform_matrix[:3, :3])\n",
"\n",
" def translate(self, vec: NDArray):\n",
" \"\"\"\n",
" translate the plane by vec\n",
" \"\"\"\n",
" assert vec.shape == (3,)\n",
" self._transform_matrix[:3, 3] += vec\n",
"\n",
" def set_transform_matrix(self, mat: NDArray):\n",
" assert mat.shape == (4, 4)\n",
" self._transform_matrix = mat"
]
},
{
"cell_type": "code",
"execution_count": 4,
"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",
"plane_b.rotate(np.pi/2, np.array([1, 0, 0]))\n",
"plane_b.rotate(np.pi, np.array([0, 0, 1]))\n",
"tmp_c = plane_b.transformed_geometry_center\n",
"plane_b.translate(-tmp_c)\n",
"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",
"tmp = plane_c.transformed_geometry_center\n",
"plane_c.translate(-tmp)\n",
"plane_c.rotate(-np.pi/2, np.array([0, 0, 1]))\n",
"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]))\n",
"plane_c.translate(np.array([0, 0, -OFFSET]))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fig = go.Figure()\n",
"t_corners_a = plane_a.transformed_corners\n",
"for i, corners in enumerate(t_corners_a):\n",
" fig.add_trace(\n",
" go.Scatter3d(\n",
" x=corners[:, 0],\n",
" y=corners[:, 1],\n",
" z=corners[:, 2],\n",
" mode=\"markers+lines+text\",\n",
" text=list(map(lambda x: f\"{plane_a.ids[i]}:{x}\", range(4))),\n",
" textposition=\"middle center\",\n",
" name=str(plane_a.ids[i]),\n",
" marker=dict(size=1),\n",
" )\n",
" )\n",
"\n",
"# normal vector\n",
"fig.add_trace(\n",
" go.Scatter3d(\n",
" x=plane_a.transformed_normal_vector[:, 0],\n",
" y=plane_a.transformed_normal_vector[:, 1],\n",
" z=plane_a.transformed_normal_vector[:, 2],\n",
" mode=\"markers+lines\",\n",
" name=\"normal_a\",\n",
" marker=dict(size=2),\n",
" )\n",
")\n",
"\n",
"t_corners_b = plane_b.transformed_corners\n",
"for i, corners in enumerate(t_corners_b):\n",
" fig.add_trace(\n",
" go.Scatter3d(\n",
" x=corners[:, 0],\n",
" y=corners[:, 1],\n",
" z=corners[:, 2],\n",
" mode=\"markers+lines+text\",\n",
" text=list(map(lambda x: f\"{plane_b.ids[i]}:{x}\", range(4))),\n",
" textposition=\"middle center\",\n",
" name=str(plane_b.ids[i]),\n",
" marker=dict(size=1),\n",
" )\n",
" )\n",
"fig.add_trace(\n",
" go.Scatter3d(\n",
" x=plane_b.transformed_normal_vector[:, 0],\n",
" y=plane_b.transformed_normal_vector[:, 1],\n",
" z=plane_b.transformed_normal_vector[:, 2],\n",
" mode=\"markers+lines\",\n",
" name=\"normal_b\",\n",
" marker=dict(size=2),\n",
" )\n",
")\n",
"\n",
"t_corners_c = plane_c.transformed_corners\n",
"for i, corners in enumerate(t_corners_c):\n",
" fig.add_trace(\n",
" go.Scatter3d(\n",
" x=corners[:, 0],\n",
" y=corners[:, 1],\n",
" z=corners[:, 2],\n",
" mode=\"markers+lines+text\",\n",
" text=list(map(lambda x: f\"{plane_c.ids[i]}:{x}\", range(4))),\n",
" name=str(plane_c.ids[i]),\n",
" marker=dict(size=1),\n",
" )\n",
" )\n",
"fig.add_trace(\n",
" go.Scatter3d(\n",
" x=plane_c.transformed_normal_vector[:, 0],\n",
" y=plane_c.transformed_normal_vector[:, 1],\n",
" z=plane_c.transformed_normal_vector[:, 2],\n",
" mode=\"markers+lines\",\n",
" textposition=\"middle center\",\n",
" name=\"normal_c\",\n",
" marker=dict(size=2),\n",
" )\n",
")\n",
"\n",
"# fig.update_layout(\n",
"# scene=dict(\n",
"# aspectmode=\"cube\",\n",
"# yaxis_autorange=\"reversed\",\n",
"# )\n",
"# )\n",
"\n",
"fig.update_layout(\n",
" scene=dict(\n",
" aspectmode='cube',\n",
" xaxis=dict(range=[-0.1, params.total_side_length]),\n",
" yaxis=dict(range=[params.total_side_length, -0.1]),\n",
" zaxis=dict(range=[-0.1, params.total_side_length]),\n",
" )\n",
")\n",
"fig.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import awkward as ak\n",
"from awkward import Record as AwkwardRecord, Array as AwkwardArray\n",
"\n",
"coords = AwkwardArray(\n",
" [\n",
" {\n",
" \"name\": \"a\",\n",
" \"ids\": plane_a.ids,\n",
" \"corners\": t_corners_a,\n",
" },\n",
" {\n",
" \"name\": \"b\",\n",
" \"ids\": plane_b.ids,\n",
" \"corners\": t_corners_b,\n",
" },\n",
" {\n",
" \"name\": \"c\",\n",
" \"ids\": plane_c.ids,\n",
" \"corners\": t_corners_c,\n",
" },\n",
" ]\n",
")\n",
"display(coords)\n",
"_ = ak.to_parquet(coords, \"output/object_points.parquet\")"
]
},
{
"cell_type": "code",
"execution_count": 7,
"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": null,
"metadata": {},
"outputs": [],
"source": [
"dict(zip(total_ids, total_corners))"
]
},
{
"cell_type": "code",
"execution_count": 9,
"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": {
"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.10"
}
},
"nbformat": 4,
"nbformat_minor": 2
}

0
new_try.ipynb → calculate_box_face_coord_naive.ipynb
View File

157
compute_3d_maybe.ipynb
View File

@ -1,157 +0,0 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [],
"source": [
"import awkward as ak\n",
"from awkward import Array as AwakwardArray, Record as AwkwardRecord\n",
"from typing import cast\n",
"import numpy as np"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<pre>[{prediction: None, trackings: [], frame_num: 0, ...},\n",
" {prediction: None, trackings: [], frame_num: 1, ...},\n",
" {prediction: None, trackings: [], frame_num: 2, ...},\n",
" {prediction: None, trackings: [], frame_num: 3, ...},\n",
" {prediction: None, trackings: [], frame_num: 4, ...},\n",
" {prediction: None, trackings: [], frame_num: 5, ...},\n",
" {prediction: None, trackings: [], frame_num: 6, ...},\n",
" {prediction: None, trackings: [], frame_num: 7, ...},\n",
" {prediction: None, trackings: [], frame_num: 8, ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" ...,\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...},\n",
" {prediction: {Akeypoints: [[...]], ...}, trackings: [{...}], ...}]\n",
"-------------------------------------------------------------------\n",
"type: 808 * {\n",
" prediction: ?{\n",
" Akeypoints: var * var * var * float64,\n",
" bboxes: var * var * float64,\n",
" scores: var * var * var * float64,\n",
" frame_number: int64,\n",
" reference_frame_size: {\n",
" &quot;0&quot;: int64,\n",
" &quot;1&quot;: int64\n",
" }\n",
" },\n",
" trackings: var * {\n",
" id: int64,\n",
" bounding_boxes: var * var * float64\n",
" },\n",
" frame_num: int64,\n",
" reference_frame_size: {\n",
" height: int64,\n",
" width: int64\n",
" }\n",
"}</pre>"
],
"text/plain": [
"<Array [{prediction: None, ...}, ..., {...}] type='808 * {prediction: ?{Ake...'>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"a_ak = ak.from_parquet(\"pose/a.parquet\")\n",
"b_ak = ak.from_parquet(\"pose/b.parquet\")\n",
"# display(a_ak)\n",
"display(b_ak)"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<pre>{Akeypoints: [[[893, 417], [898, 408], [...], ..., [782, 596], [785, 599]]],\n",
" bboxes: [[756, 341, 940, 597]],\n",
" scores: [[[0.907], [0.896], [0.916], [0.341], ..., [0.811], [0.835], [0.802]]],\n",
" frame_number: 5,\n",
" reference_frame_size: {&#x27;0&#x27;: 1080, &#x27;1&#x27;: 1920}}\n",
"--------------------------------------------------------------------------------\n",
"type: {\n",
" Akeypoints: var * var * var * float64,\n",
" bboxes: var * var * float64,\n",
" scores: var * var * var * float64,\n",
" frame_number: int64,\n",
" reference_frame_size: {\n",
" &quot;0&quot;: int64,\n",
" &quot;1&quot;: int64\n",
" }\n",
"}</pre>"
],
"text/plain": [
"<Record {Akeypoints: [[...]], bboxes: ..., ...} type='{Akeypoints: var * va...'>"
]
},
"execution_count": 24,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"a_ak[\"prediction\"][5]"
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {},
"outputs": [],
"source": [
"unique_tracking_ids_a = np.unique(ak.to_numpy(ak.flatten(cast(AwakwardArray, a_ak[\"trackings\"][\"id\"]))))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"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
}

4
draw_uv.ipynb
View File

@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
"execution_count": 38,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@ -99,7 +99,7 @@
},
{
"cell_type": "code",
"execution_count": 41,
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [

230
estimate_extrinstic.ipynb Normal file
View File

@ -0,0 +1,230 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import cv2\n",
"import cv2.aruco as aruco\n",
"from typing import Sequence, cast\n",
"import awkward as ak\n",
"from pathlib import Path\n",
"import numpy as np\n",
"from typing import Final\n",
"from matplotlib import pyplot as plt\n",
"from cv2.typing import MatLike"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"A_PATH = Path(\"output/af_03.parquet\")\n",
"B_PATH = Path(\"output/ae_08.parquet\")\n",
"\n",
"a_params = ak.from_parquet(A_PATH)[0]\n",
"b_params = ak.from_parquet(B_PATH)[0]\n",
"display(a_params)\n",
"display(b_params)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def create_new_aruco_marker_origin(marker_length: float):\n",
" \"\"\"\n",
" Create a new ArUco marker origin with the given length.\n",
"\n",
" 0 -> x\n",
" |\n",
" v\n",
" y\n",
"\n",
" 0---1\n",
" | |\n",
" 3---2\n",
"\n",
" So that the center of the marker is the origin for this PnP problem.\n",
"\n",
" Args:\n",
" marker_length: The length of the marker.\n",
" \"\"\"\n",
" return np.array(\n",
" [\n",
" [-marker_length / 2, marker_length / 2, 0],\n",
" [marker_length / 2, marker_length / 2, 0],\n",
" [marker_length / 2, -marker_length / 2, 0],\n",
" [-marker_length / 2, -marker_length / 2, 0],\n",
" ]\n",
" ).astype(np.float32)\n",
"\n",
"\n",
"DICTIONARY: Final[int] = aruco.DICT_4X4_50\n",
"# 400mm\n",
"MARKER_LENGTH: Final[float] = 0.4\n",
"aruco_dict = aruco.getPredefinedDictionary(DICTIONARY)\n",
"detector = aruco.ArucoDetector(\n",
" dictionary=aruco_dict, detectorParams=aruco.DetectorParameters()\n",
")"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"a_img = cv2.imread(str(Path(\"dumped/marker/video-20241205-152716-board.png\")))\n",
"a_mtx = ak.to_numpy(a_params[\"camera_matrix\"])\n",
"a_dist = ak.to_numpy(a_params[\"distortion_coefficients\"])\n",
"\n",
"b_img = cv2.imread(str(Path(\"dumped/marker/video-20241205-152721-board.png\")))\n",
"b_mtx = ak.to_numpy(b_params[\"camera_matrix\"])\n",
"b_dist = ak.to_numpy(b_params[\"distortion_coefficients\"])"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"a_corners, a_ids, _a_rejected = detector.detectMarkers(a_img)\n",
"b_corners, b_ids, _b_rejected = detector.detectMarkers(b_img)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"a_corners"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"ok, a_rvec, a_tvec = cv2.solvePnP(create_new_aruco_marker_origin(MARKER_LENGTH), a_corners[0], a_mtx, a_dist)\n",
"if not ok:\n",
" raise ValueError(\"Failed to solve PnP for A\")\n",
"a_img_output = cv2.drawFrameAxes(a_img, a_mtx, a_dist, a_rvec, a_tvec, MARKER_LENGTH)\n",
"plt.imshow(cv2.cvtColor(a_img_output, cv2.COLOR_BGR2RGB))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"ok, b_rvec, b_tvec = cv2.solvePnP(create_new_aruco_marker_origin(MARKER_LENGTH), b_corners[0], b_mtx, b_dist)\n",
"if not ok:\n",
" raise ValueError(\"Failed to solve PnP for B\")\n",
"b_img_output = cv2.drawFrameAxes(b_img, b_mtx, b_dist, b_rvec, b_tvec, MARKER_LENGTH)\n",
"plt.imshow(cv2.cvtColor(b_img_output, cv2.COLOR_BGR2RGB))"
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [],
"source": [
"from typing import TypeVar, Union\n",
"\n",
"\n",
"T = TypeVar(\"T\")\n",
"\n",
"\n",
"def create_transform_matrix(rvec: MatLike, tvec: MatLike, dtype: type = np.float32):\n",
" assert rvec.shape == (3, 1)\n",
" assert tvec.shape == (3, 1)\n",
" R, _ = cv2.Rodrigues(rvec)\n",
" transform = np.eye(4, dtype=dtype)\n",
" transform[:3, :3] = R\n",
" transform[:3, 3] = tvec.flatten()\n",
" return transform\n",
"\n",
"\n",
"def extract_translation(transform: MatLike):\n",
" assert transform.shape == (4, 4)\n",
" return transform[:3, 3]\n",
"\n",
"\n",
"def extract_rotation(transform: MatLike):\n",
" assert transform.shape == (4, 4)\n",
" return transform[:3, :3]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"a_trans = create_transform_matrix(a_rvec, a_tvec)\n",
"display(a_trans)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"np.linalg.inv(a_trans)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Converts a rotation matrix to a rotation vector or vice versa\n",
"a_rmtx, _ = cv2.Rodrigues(a_rvec)\n",
"b_rmtx, _ = cv2.Rodrigues(b_rvec)\n",
"a_camera_coord = -(a_rmtx.T@ a_tvec)\n",
"b_camera_coord = -(b_rmtx.T @ b_tvec)\n",
"distance = np.linalg.norm(a_camera_coord - b_camera_coord)\n",
"a_distance = np.linalg.norm(a_camera_coord)\n",
"b_distance = np.linalg.norm(b_camera_coord)\n",
"display(\"d_ab={:.4}m a={:.4}m b={:.4}m\".format(distance, a_distance, b_distance))\n",
"display(\"a_coord={}\".format(a_camera_coord.T))\n",
"display(\"b_coord={}\".format(b_camera_coord.T))"
]
}
],
"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.10"
}
},
"nbformat": 4,
"nbformat_minor": 2
}

0
find_cute_box.py → find_aruco_points.py
View File

49
find_cute_box_with_image.ipynb → find_aruco_points_with_image.ipynb
View File

@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
"execution_count": 117,
"execution_count": 14,
"metadata": {},
"outputs": [],
"source": [
@ -29,7 +29,7 @@
},
{
"cell_type": "code",
"execution_count": 118,
"execution_count": 15,
"metadata": {},
"outputs": [],
"source": [
@ -42,7 +42,7 @@
},
{
"cell_type": "code",
"execution_count": 119,
"execution_count": 16,
"metadata": {},
"outputs": [],
"source": [
@ -54,7 +54,7 @@
},
{
"cell_type": "code",
"execution_count": 120,
"execution_count": 17,
"metadata": {},
"outputs": [],
"source": [
@ -66,7 +66,7 @@
},
{
"cell_type": "code",
"execution_count": 121,
"execution_count": 18,
"metadata": {},
"outputs": [],
"source": [
@ -88,7 +88,7 @@
},
{
"cell_type": "code",
"execution_count": 122,
"execution_count": 19,
"metadata": {},
"outputs": [],
"source": [
@ -121,7 +121,7 @@
},
{
"cell_type": "code",
"execution_count": 123,
"execution_count": 20,
"metadata": {},
"outputs": [],
"source": [
@ -192,7 +192,7 @@
},
{
"cell_type": "code",
"execution_count": 126,
"execution_count": 23,
"metadata": {},
"outputs": [],
"source": [
@ -304,7 +304,7 @@
},
{
"cell_type": "code",
"execution_count": 131,
"execution_count": 25,
"metadata": {},
"outputs": [],
"source": [
@ -324,6 +324,22 @@
"outputs": [],
"source": [
"from functools import lru_cache\n",
"from typing import Optional, TypedDict\n",
"import awkward as ak\n",
"\n",
"\n",
"class MarkerFace(TypedDict):\n",
" name: str\n",
" ids: Int[NDArray, \"N\"]\n",
" \"\"\"\n",
" ArUco marker ids\n",
" \"\"\"\n",
" corners: Num[NDArray, \"N 4 3\"]\n",
" \"\"\"\n",
" Corner coordinates in 3D of rectangle,\n",
" relative to the world origin\n",
" \"\"\"\n",
"\n",
"\n",
"@dataclass\n",
"class Face:\n",
@ -349,6 +365,21 @@
" \"left\": Face(color=hex_to_rgb(colors[5]), marker_ids=[20, 21, 22, 23]),\n",
"}\n",
"\n",
"markers: list[MarkerFace] = []\n",
"for name, face in faces.items():\n",
" corners = np.array([id_to_3d_coords[id] for id in face.marker_ids])\n",
" assert corners.shape == (4, 4, 3)\n",
" markers.append(MarkerFace(name=name, ids=np.array(face.marker_ids), corners=corners))\n",
"display(markers)\n",
"ak.to_parquet(markers, \"output/standard_box_markers.parquet\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"@lru_cache\n",
"def get_face_by_marker_id(marker_id: int) -> Optional[Face]:\n",
" for face in faces.values():\n",

35
find_cute_object.py → find_extrinsic_object.py
View File

@ -1,21 +1,44 @@
import cv2
from cv2 import aruco
from datetime import datetime
from loguru import logger
from pathlib import Path
from typing import Optional, cast, Final
from typing import Final, Optional, TypedDict, cast
import awkward as ak
from cv2.typing import MatLike
import cv2
import numpy as np
from cv2 import aruco
from cv2.typing import MatLike
from jaxtyping import Int, Num
from loguru import logger
NDArray = np.ndarray
CALIBRATION_PARQUET = Path("output") / "usbcam_cal.parquet"
OBJECT_POINTS_PARQUET = Path("output") / "object_points.parquet"
# OBJECT_POINTS_PARQUET = Path("output") / "object_points.parquet"
OBJECT_POINTS_PARQUET = Path("output") / "standard_box_markers.parquet"
DICTIONARY: Final[int] = aruco.DICT_4X4_50
# 400mm
MARKER_LENGTH: Final[float] = 0.4
class MarkerFace(TypedDict):
"""
for diamond ArUco markers, N is 4
"""
name: str
"""
a label for the face
"""
ids: Int[NDArray, "N"]
"""
ArUco marker ids
"""
corners: Num[NDArray, "N 4 3"]
"""
Corner coordinates in 3D of rectangle,
relative to the world origin
"""
def gen():
API = cv2.CAP_AVFOUNDATION
cap = cv2.VideoCapture(0, API)

BIN
output/object_points.parquet LFS Normal file
View File

Binary file not shown.

BIN
output/standard_box_markers.parquet LFS Normal file
View File

Binary file not shown.

422
play.ipynb
View File

File diff suppressed because one or more lines are too long

53
playground.py Normal file
View File

@ -0,0 +1,53 @@
# ---
# jupyter:
# jupytext:
# text_representation:
# extension: .py
# format_name: percent
# format_version: '1.3'
# jupytext_version: 1.17.0
# kernelspec:
# language: python
# name: python3
# ---
# %%
import awkward as ak
from pathlib import Path
import numpy as np
from IPython.display import display
from typing import TypedDict
from jaxtyping import Int, Num
NDArray = np.ndarray
# %%
class MarkerFace(TypedDict):
"""
for diamond ArUco markers, N is 4
"""
name: str
"""
a label for the face
"""
ids: Int[NDArray, "N"]
"""
ArUco marker ids
"""
corners: Num[NDArray, "N 4 3"]
"""
Corner coordinates in 3D of rectangle,
relative to the world origin
"""
# %%
# OBJECT_POINTS_PARQUET = Path("output") / "object_points.parquet"
OBJECT_POINTS_PARQUET = Path("output") / "standard_box_markers.parquet"
ops = ak.from_parquet(OBJECT_POINTS_PARQUET)
display(ops)
# %%

109
get_ext.ipynb → preprocess_camera_parameters.ipynb
View File

@ -181,18 +181,9 @@
},
{
"cell_type": "code",
"execution_count": 43,
"execution_count": null,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/var/folders/cj/0zmvpygn7m72m42lh6x_hcgw0000gn/T/ipykernel_79393/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"
]
}
],
"outputs": [],
"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))"
@ -200,18 +191,9 @@
},
{
"cell_type": "code",
"execution_count": 44,
"execution_count": null,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/var/folders/cj/0zmvpygn7m72m42lh6x_hcgw0000gn/T/ipykernel_79393/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"
]
}
],
"outputs": [],
"source": [
"b_result_img, b_rvec, b_tvec = process(b_img, b_mtx, b_dist)\n",
"# plt.imshow(cv2.cvtColor(b_result_img, cv2.COLOR_BGR2RGB))"
@ -219,18 +201,9 @@
},
{
"cell_type": "code",
"execution_count": 45,
"execution_count": null,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/var/folders/cj/0zmvpygn7m72m42lh6x_hcgw0000gn/T/ipykernel_79393/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"
]
}
],
"outputs": [],
"source": [
"c_result_img, c_rvec, c_tvec = process(c_img, c_mtx, c_dist)\n",
"c_prime_result_img, c_prime_rvec, c_prime_tvec = process(c_prime_img, c_mtx, c_dist)"
@ -238,58 +211,9 @@
},
{
"cell_type": "code",
"execution_count": 46,
"execution_count": null,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"'params'"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/html": [
"<pre>[{name: &#x27;a-ae_08&#x27;, rvec: [[-0.602], ..., [-3.05]], tvec: [...], ...},\n",
" {name: &#x27;b-ae_09&#x27;, rvec: [[0.572], ..., [3.02]], tvec: [...], ...},\n",
" {name: &#x27;c-af_03&#x27;, rvec: [[-1.98], ..., [-2.4]], tvec: [...], ...},\n",
" {name: &#x27;c-prime-af_03&#x27;, rvec: [[-1.99], ...], tvec: [...], ...}]\n",
"---------------------------------------------------------------------\n",
"type: 4 * {\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-ae_08', rvec: ..., ...}, ...] type='4 * {name: string, rv...'>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/plain": [
"<pyarrow._parquet.FileMetaData object at 0x311da8900>\n",
" created_by: parquet-cpp-arrow version 14.0.1\n",
" num_columns: 5\n",
" num_rows: 4\n",
" num_row_groups: 1\n",
" format_version: 2.6\n",
" serialized_size: 0"
]
},
"execution_count": 46,
"metadata": {},
"output_type": "execute_result"
}
],
"outputs": [],
"source": [
"params = AwkwardArray(\n",
" [\n",
@ -329,20 +253,9 @@
},
{
"cell_type": "code",
"execution_count": 47,
"execution_count": null,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"True"
]
},
"execution_count": 47,
"metadata": {},
"output_type": "execute_result"
}
],
"outputs": [],
"source": [
"cv2.imwrite(\"output/a_result_img.png\", a_result_img)\n",
"cv2.imwrite(\"output/b_result_img.png\", b_result_img)\n",
@ -367,7 +280,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.8"
"version": "3.12.10"
}
},
"nbformat": 4,