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

fixing

Browse Source
This commit is contained in:
crosstyan
2024-12-18 10:19:21 +08:00
parent 0953fbdf9e
commit 1d086ac20f
1 changed files with 143 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
boom.ipynb
View File

@ -2,7 +2,7 @@
"cells": [ "cells": [
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 255, "execution_count": 43,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -54,6 +54,7 @@
"class ArUcoMarker2D:\n", "class ArUcoMarker2D:\n",
" id: int\n", " id: int\n",
" corners: Quad2D\n", " corners: Quad2D\n",
" params: DiamondBoardParameter\n",
"\n", "\n",
" @property\n", " @property\n",
" def np_corners(self):\n", " def np_corners(self):\n",
@ -108,16 +109,16 @@
" params.border_length + params.chess_length * 2 + params.marker_border_length\n", " params.border_length + params.chess_length * 2 + params.marker_border_length\n",
" )\n", " )\n",
" return (\n", " return (\n",
" ArUcoMarker2D(ids[0], tl_to_square(tl_0_x, tl_0_y, params.marker_leghth)),\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)),\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)),\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)),\n", " ArUcoMarker2D(ids[3], tl_to_square(tl_3_x, tl_3_y, params.marker_leghth), params),\n",
" )\n" " )\n"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 256, "execution_count": 44,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -138,7 +139,7 @@
"fig = plt.figure()\n", "fig = plt.figure()\n",
"ax = fig.gca()\n", "ax = fig.gca()\n",
"ax.set_xlim((0, params.total_side_length))\n", "ax.set_xlim((0, params.total_side_length))\n",
"ax.set_ylim((0, params.total_side_length))\n", "ax.set_ylim((0, params.total_side_length)) # type: ignore\n",
"ax.set_aspect(\"equal\")\n", "ax.set_aspect(\"equal\")\n",
"# set origin to top-left (from bottom-left)\n", "# set origin to top-left (from bottom-left)\n",
"ax.invert_yaxis()\n", "ax.invert_yaxis()\n",
@ -154,7 +155,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 257, "execution_count": 45,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -192,6 +193,11 @@
" \"\"\"\n", " \"\"\"\n",
" 4x4 transformation matrix\n", " 4x4 transformation matrix\n",
" \"\"\"\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", "\n",
" def __init__(self, items: Sequence[ArUcoMarker2D]):\n", " def __init__(self, items: Sequence[ArUcoMarker2D]):\n",
" self._ids = np.array([item.id for item in items])\n", " self._ids = np.array([item.id for item in items])\n",
@ -202,6 +208,11 @@
" [corners_2d, np.zeros((corners_2d.shape[0], 4, 1))], axis=-1\n", " [corners_2d, np.zeros((corners_2d.shape[0], 4, 1))], axis=-1\n",
" )\n", " )\n",
" self._transform_matrix = np.eye(4)\n", " self._transform_matrix = np.eye(4)\n",
" def center(items: Sequence[ArUcoMarker2D]):\n",
" return np.mean([item.np_corners for item in items], axis=(0, 1))\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", "\n",
" @property\n", " @property\n",
" def ids(self):\n", " def ids(self):\n",
@ -223,6 +234,13 @@
" return np.array(list(g()))\n", " return np.array(list(g()))\n",
" \n", " \n",
" @property\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",
" return np.array(list(g()))\n",
" \n",
" @property\n",
" def transformed_geometry_center(self):\n", " def transformed_geometry_center(self):\n",
" return np.mean(self.transformed_corners, axis=(0, 1))\n", " return np.mean(self.transformed_corners, axis=(0, 1))\n",
" \n", " \n",
@ -261,19 +279,19 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 258, "execution_count": 46,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"plane = DiamondPlane3D(markers)\n", "plane_a = DiamondPlane3D(markers)\n",
"# plane.rotate(-np.pi/2, np.array([0, 1, 0]))\n", "# plane.rotate(-np.pi/2, np.array([0, 1, 0]))\n",
"# t_corners = plane.corners\n", "# t_corners = plane.corners\n",
"t_corners = plane.transformed_corners" "t_corners = plane_a.transformed_corners"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 259, "execution_count": 47,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -298,9 +316,17 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 49,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[0.2005 0.20049998 0. ]\n",
" [0.2005 0.20049998 0.1 ]]\n"
]
},
{ {
"data": { "data": {
"application/vnd.plotly.v1+json": { "application/vnd.plotly.v1+json": {
@ -732,6 +758,26 @@
0 0
] ]
}, },
{
"marker": {
"size": 2
},
"mode": "markers+lines",
"name": "normal_a",
"type": "scatter3d",
"x": [
0.2004999965429306,
0.2004999965429306
],
"y": [
0.2004999816417694,
0.2004999816417694
],
"z": [
0,
0.1
]
},
{ {
"marker": { "marker": {
"size": 1 "size": 1
@ -1156,6 +1202,26 @@
0.0350000128746033 0.0350000128746033
] ]
}, },
{
"marker": {
"size": 2
},
"mode": "markers+lines",
"name": "normal_b",
"type": "scatter3d",
"x": [
0.2004999965429306,
0.2004999965429306
],
"y": [
1.2277083037334799e-17,
0.10000000000000002
],
"z": [
0.21050001835823062,
0.21050001835823062
]
},
{ {
"marker": { "marker": {
"size": 1 "size": 1
@ -1579,6 +1645,26 @@
"z": [ "z": [
0.03500001287460331 0.03500001287460331
] ]
},
{
"marker": {
"size": 2
},
"mode": "markers+lines",
"name": "normal_c",
"type": "scatter3d",
"x": [
-1.2277084862200736e-17,
0.09999999999999999
],
"y": [
0.21050000345706943,
0.21050000345706943
],
"z": [
0.21050001835823062,
0.21050001835823065
]
} }
], ],
"layout": { "layout": {
@ -1586,19 +1672,19 @@
"aspectmode": "cube", "aspectmode": "cube",
"xaxis": { "xaxis": {
"range": [ "range": [
-0.1, -0.41100000000000003,
0.41100000000000003 0.41100000000000003
] ]
}, },
"yaxis": { "yaxis": {
"range": [ "range": [
-0.1, -0.41100000000000003,
0.41100000000000003 0.41100000000000003
] ]
}, },
"zaxis": { "zaxis": {
"range": [ "range": [
-0.1, -0.41100000000000003,
0.41100000000000003 0.41100000000000003
] ]
} }
@ -2434,10 +2520,9 @@
" y=corners[:, 1],\n", " y=corners[:, 1],\n",
" z=corners[:, 2],\n", " z=corners[:, 2],\n",
" mode='markers+lines',\n", " mode='markers+lines',\n",
" name=str(plane.ids[i]),\n", " name=str(plane_a.ids[i]),\n",
" marker=dict(size=1),\n", " marker=dict(size=1),\n",
" ))\n", " ))\n",
" # also draw the corner id\n",
" for j, (x, y, z) in enumerate(corners):\n", " for j, (x, y, z) in enumerate(corners):\n",
" fig.add_trace(go.Scatter3d(\n", " fig.add_trace(go.Scatter3d(\n",
" x=[x],\n", " x=[x],\n",
@ -2448,6 +2533,18 @@
" textposition='middle center',\n", " textposition='middle center',\n",
" marker=dict(size=5),\n", " marker=dict(size=5),\n",
" ))\n", " ))\n",
"\n",
"print(plane_a.transformed_normal_vector)\n",
"# normal vector\n",
"fig.add_trace(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",
"for i, corners in enumerate(t_corners_b):\n", "for i, corners in enumerate(t_corners_b):\n",
" fig.add_trace(go.Scatter3d(\n", " fig.add_trace(go.Scatter3d(\n",
" x=corners[:, 0],\n", " x=corners[:, 0],\n",
@ -2467,6 +2564,14 @@
" textposition='middle center',\n", " textposition='middle center',\n",
" marker=dict(size=5),\n", " marker=dict(size=5),\n",
" ))\n", " ))\n",
"fig.add_trace(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",
"for i, corners in enumerate(t_corners_c):\n", "for i, corners in enumerate(t_corners_c):\n",
" fig.add_trace(go.Scatter3d(\n", " fig.add_trace(go.Scatter3d(\n",
@ -2488,25 +2593,32 @@
" textposition='middle center',\n", " textposition='middle center',\n",
" marker=dict(size=5),\n", " marker=dict(size=5),\n",
" ))\n", " ))\n",
"fig.add_trace(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",
" name='normal_c',\n",
" marker=dict(size=2),\n",
"))\n",
"\n", "\n",
"\n",
"# fig.update_layout(\n",
"# scene=dict(\n",
"# aspectmode='cube',\n",
"# xaxis=dict(range=[-params.total_side_length, params.total_side_length]),\n",
"# yaxis=dict(range=[-params.total_side_length, params.total_side_length]),\n",
"# zaxis=dict(range=[-params.total_side_length, params.total_side_length]),\n",
"# # zaxis=dict(range=[0, params.total_side_length]),\n",
"# )\n",
"# )\n",
"fig.update_layout(\n", "fig.update_layout(\n",
" scene=dict(\n", " scene=dict(\n",
" aspectmode='cube',\n", " aspectmode='cube',\n",
" xaxis=dict(range=[-0.1, params.total_side_length]),\n", " xaxis=dict(range=[-params.total_side_length, params.total_side_length]),\n",
" yaxis=dict(range=[-0.1, params.total_side_length]),\n", " yaxis=dict(range=[-params.total_side_length, params.total_side_length]),\n",
" zaxis=dict(range=[-0.1, params.total_side_length]),\n", " zaxis=dict(range=[-params.total_side_length, params.total_side_length]),\n",
" # zaxis=dict(range=[0, params.total_side_length]),\n",
" )\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=[-0.1, params.total_side_length]),\n",
"# zaxis=dict(range=[-0.1, params.total_side_length]),\n",
"# )\n",
"# )\n",
"fig.show()" "fig.show()"
] ]
} }