fixing
This commit is contained in:
174
boom.ipynb
174
boom.ipynb
@ -2,7 +2,7 @@
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 255,
|
||||
"execution_count": 43,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
@ -54,6 +54,7 @@
|
||||
"class ArUcoMarker2D:\n",
|
||||
" id: int\n",
|
||||
" corners: Quad2D\n",
|
||||
" params: DiamondBoardParameter\n",
|
||||
"\n",
|
||||
" @property\n",
|
||||
" def np_corners(self):\n",
|
||||
@ -108,16 +109,16 @@
|
||||
" 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)),\n",
|
||||
" ArUcoMarker2D(ids[1], tl_to_square(tl_1_x, tl_1_y, params.marker_leghth)),\n",
|
||||
" ArUcoMarker2D(ids[2], tl_to_square(tl_2_x, tl_2_y, params.marker_leghth)),\n",
|
||||
" ArUcoMarker2D(ids[3], tl_to_square(tl_3_x, tl_3_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), 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": 256,
|
||||
"execution_count": 44,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
@ -138,7 +139,7 @@
|
||||
"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))\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",
|
||||
@ -154,7 +155,7 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 257,
|
||||
"execution_count": 45,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
@ -192,6 +193,11 @@
|
||||
" \"\"\"\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",
|
||||
@ -202,6 +208,11 @@
|
||||
" [corners_2d, np.zeros((corners_2d.shape[0], 4, 1))], axis=-1\n",
|
||||
" )\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",
|
||||
" @property\n",
|
||||
" def ids(self):\n",
|
||||
@ -223,6 +234,13 @@
|
||||
" 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",
|
||||
" 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",
|
||||
@ -261,19 +279,19 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 258,
|
||||
"execution_count": 46,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"plane = DiamondPlane3D(markers)\n",
|
||||
"plane_a = DiamondPlane3D(markers)\n",
|
||||
"# plane.rotate(-np.pi/2, np.array([0, 1, 0]))\n",
|
||||
"# t_corners = plane.corners\n",
|
||||
"t_corners = plane.transformed_corners"
|
||||
"t_corners = plane_a.transformed_corners"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 259,
|
||||
"execution_count": 47,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
@ -298,9 +316,17 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 49,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"[[0.2005 0.20049998 0. ]\n",
|
||||
" [0.2005 0.20049998 0.1 ]]\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"application/vnd.plotly.v1+json": {
|
||||
@ -732,6 +758,26 @@
|
||||
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": {
|
||||
"size": 1
|
||||
@ -1156,6 +1202,26 @@
|
||||
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": {
|
||||
"size": 1
|
||||
@ -1579,6 +1645,26 @@
|
||||
"z": [
|
||||
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": {
|
||||
@ -1586,19 +1672,19 @@
|
||||
"aspectmode": "cube",
|
||||
"xaxis": {
|
||||
"range": [
|
||||
-0.1,
|
||||
-0.41100000000000003,
|
||||
0.41100000000000003
|
||||
]
|
||||
},
|
||||
"yaxis": {
|
||||
"range": [
|
||||
-0.1,
|
||||
-0.41100000000000003,
|
||||
0.41100000000000003
|
||||
]
|
||||
},
|
||||
"zaxis": {
|
||||
"range": [
|
||||
-0.1,
|
||||
-0.41100000000000003,
|
||||
0.41100000000000003
|
||||
]
|
||||
}
|
||||
@ -2434,10 +2520,9 @@
|
||||
" y=corners[:, 1],\n",
|
||||
" z=corners[:, 2],\n",
|
||||
" mode='markers+lines',\n",
|
||||
" name=str(plane.ids[i]),\n",
|
||||
" name=str(plane_a.ids[i]),\n",
|
||||
" marker=dict(size=1),\n",
|
||||
" ))\n",
|
||||
" # also draw the corner id\n",
|
||||
" for j, (x, y, z) in enumerate(corners):\n",
|
||||
" fig.add_trace(go.Scatter3d(\n",
|
||||
" x=[x],\n",
|
||||
@ -2448,6 +2533,18 @@
|
||||
" textposition='middle center',\n",
|
||||
" marker=dict(size=5),\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",
|
||||
" fig.add_trace(go.Scatter3d(\n",
|
||||
" x=corners[:, 0],\n",
|
||||
@ -2467,6 +2564,14 @@
|
||||
" textposition='middle center',\n",
|
||||
" marker=dict(size=5),\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",
|
||||
"for i, corners in enumerate(t_corners_c):\n",
|
||||
" fig.add_trace(go.Scatter3d(\n",
|
||||
@ -2488,25 +2593,32 @@
|
||||
" textposition='middle center',\n",
|
||||
" marker=dict(size=5),\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",
|
||||
"# 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",
|
||||
" 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",
|
||||
" 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",
|
||||
"# 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()"
|
||||
]
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user