diff --git a/boom.ipynb b/boom.ipynb index d63f2b5..e57865e 100644 --- a/boom.ipynb +++ b/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", - " \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", - "# 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()" ] }