diff --git a/boom.ipynb b/boom.ipynb index 74bad6a..08a7f59 100644 --- a/boom.ipynb +++ b/boom.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 50, + "execution_count": 45, "metadata": {}, "outputs": [], "source": [ @@ -118,7 +118,7 @@ }, { "cell_type": "code", - "execution_count": 51, + "execution_count": 46, "metadata": {}, "outputs": [ { @@ -155,7 +155,7 @@ }, { "cell_type": "code", - "execution_count": 52, + "execution_count": 47, "metadata": {}, "outputs": [], "source": [ @@ -208,8 +208,10 @@ " [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", @@ -230,33 +232,36 @@ " def transformed_corners(self):\n", " def g():\n", " for corner in self.corners:\n", - " yield np.array([transform_point(self.transform_matrix, c) for c in corner])\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", + "\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", + "\n", " @property\n", " def transformed_geometry_center(self):\n", " return np.mean(self.transformed_corners, axis=(0, 1))\n", - " \n", + "\n", " def local_rotate(self, angle: float, axis: NDArray):\n", " \"\"\"\n", " rotate the plane by angle (in radian) around local center\n", - " \n", + "\n", " Args:\n", " angle: in radian\n", " axis: (3,)\n", - " \n", + "\n", " change basis to local basis, rotate, then change back\n", " \"\"\"\n", " raise NotImplementedError\n", - " \n", - " \n", + "\n", " def rotate(self, angle: float, axis: NDArray):\n", " \"\"\"\n", " rotate the plane by angle (in radian) around the axis\n", @@ -279,45 +284,24 @@ }, { "cell_type": "code", - "execution_count": 53, + "execution_count": 48, "metadata": {}, "outputs": [], "source": [ "plane_a = DiamondPlane3D(markers)\n", - "# plane.rotate(-np.pi/2, np.array([0, 1, 0]))\n", - "# t_corners = plane.corners\n", - "t_corners = plane_a.transformed_corners" - ] - }, - { - "cell_type": "code", - "execution_count": 54, - "metadata": {}, - "outputs": [], - "source": [ + "\n", "markers_b = generate_diamond_corners((20, 21, 22, 23), params)\n", "plane_b = DiamondPlane3D(markers_b)\n", - "# plane_b.rotate(-np.pi / 2, np.array([1, 0, 0]))\n", "plane_b.translate(np.array([0, 0, 0.1]))\n", - "t_corners_b = plane_b.transformed_corners\n", "\n", "markers_c = generate_diamond_corners((24, 25, 26, 27), params)\n", "plane_c = DiamondPlane3D(markers_c)\n", - "plane_c.translate(np.array([0, 0, 0.2]))\n", - "# plane_c.rotate(np.pi/2, np.array([0, 0, 1]))\n", - "# # plane_c.translate(np.array([params.total_side_length, 0, 0]))\n", - "# plane_c.rotate(-np.pi/2, np.array([0, 1, 0]))\n", - "# plane_c.translate(np.array([params.total_side_length/2, 0, 0]))\n", - "# plane_c.rotate(np.pi, np.array([0, 0, 1]))\n", - "# plane_c.translate(np.array([-params.total_side_length/2, params.total_side_length, params.total_side_length]))\n", - "# plane_c.rotate(np.pi/2, np.array([0, 0, 1]))\n", - "# plane_c.translate(np.array([0, 0, params.total_side_length]))\n", - "t_corners_c = plane_c.transformed_corners" + "plane_c.translate(np.array([0, 0, 0.2]))" ] }, { "cell_type": "code", - "execution_count": 55, + "execution_count": 49, "metadata": {}, "outputs": [ { @@ -339,8 +323,9 @@ "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "16", + "text": "16", "type": "scatter3d", "x": [ 0.15199999511241913, @@ -361,92 +346,13 @@ 0 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.02500000037252903 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.02500000037252903 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.12200000137090683 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.12200000137090683 - ], - "z": [ - 0 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "17", + "text": "17", "type": "scatter3d", "x": [ 0.02500000037252903, @@ -467,92 +373,13 @@ 0 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.02500000037252903 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.12200000137090683 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.12200000137090683 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.02500000037252903 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "18", + "text": "18", "type": "scatter3d", "x": [ 0.27900001406669617, @@ -573,92 +400,13 @@ 0 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.27900001406669617 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.37599998712539673 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.37599998712539673 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.27900001406669617 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "19", + "text": "19", "type": "scatter3d", "x": [ 0.15199999511241913, @@ -679,86 +427,6 @@ 0 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.27900001406669617 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.27900001406669617 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.37599998712539673 - ], - "z": [ - 0 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.37599998712539673 - ], - "z": [ - 0 - ] - }, { "marker": { "size": 2 @@ -783,8 +451,9 @@ "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "20", + "text": "20", "type": "scatter3d", "x": [ 0.15199999511241913, @@ -805,92 +474,13 @@ 0.1 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.02500000037252903 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.02500000037252903 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.12200000137090683 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.12200000137090683 - ], - "z": [ - 0.1 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "21", + "text": "21", "type": "scatter3d", "x": [ 0.02500000037252903, @@ -911,92 +501,13 @@ 0.1 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.02500000037252903 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.12200000137090683 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.12200000137090683 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.02500000037252903 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.1 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "22", + "text": "22", "type": "scatter3d", "x": [ 0.27900001406669617, @@ -1017,92 +528,13 @@ 0.1 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.27900001406669617 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.37599998712539673 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.37599998712539673 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.27900001406669617 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.1 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "23", + "text": "23", "type": "scatter3d", "x": [ 0.15199999511241913, @@ -1123,86 +555,6 @@ 0.1 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.27900001406669617 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.27900001406669617 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.37599998712539673 - ], - "z": [ - 0.1 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.37599998712539673 - ], - "z": [ - 0.1 - ] - }, { "marker": { "size": 2 @@ -1227,8 +579,9 @@ "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "24", + "text": "24", "type": "scatter3d", "x": [ 0.15199999511241913, @@ -1249,92 +602,13 @@ 0.2 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.02500000037252903 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.02500000037252903 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.12200000137090683 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.12200000137090683 - ], - "z": [ - 0.2 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "25", + "text": "25", "type": "scatter3d", "x": [ 0.02500000037252903, @@ -1355,92 +629,13 @@ 0.2 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.02500000037252903 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.12200000137090683 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.12200000137090683 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.02500000037252903 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.2 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "26", + "text": "26", "type": "scatter3d", "x": [ 0.27900001406669617, @@ -1461,92 +656,13 @@ 0.2 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.27900001406669617 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.37599998712539673 - ], - "y": [ - 0.15199999511241913 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.37599998712539673 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.27900001406669617 - ], - "y": [ - 0.24899999797344208 - ], - "z": [ - 0.2 - ] - }, { "marker": { "size": 1 }, - "mode": "markers+lines", + "mode": "markers+lines+text", "name": "27", + "text": "27", "type": "scatter3d", "x": [ 0.15199999511241913, @@ -1567,86 +683,6 @@ 0.2 ] }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "0" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.27900001406669617 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "1" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.27900001406669617 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "2" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.24899999797344208 - ], - "y": [ - 0.37599998712539673 - ], - "z": [ - 0.2 - ] - }, - { - "marker": { - "size": 5 - }, - "mode": "markers+text", - "text": [ - "3" - ], - "textposition": "middle center", - "type": "scatter3d", - "x": [ - 0.15199999511241913 - ], - "y": [ - 0.37599998712539673 - ], - "z": [ - 0.2 - ] - }, { "marker": { "size": 2 @@ -1679,8 +715,8 @@ }, "yaxis": { "range": [ - -0.41100000000000003, - 0.41100000000000003 + 0.41100000000000003, + -0.41100000000000003 ] }, "zaxis": { @@ -2515,108 +1551,96 @@ ], "source": [ "fig = go.Figure()\n", + "t_corners = plane_a.transformed_corners\n", "for i, corners in enumerate(t_corners):\n", - " fig.add_trace(go.Scatter3d(\n", - " x=corners[:, 0],\n", - " y=corners[:, 1],\n", - " z=corners[:, 2],\n", - " mode='markers+lines',\n", - " name=str(plane_a.ids[i]),\n", - " marker=dict(size=1),\n", - " ))\n", - " for j, (x, y, z) in enumerate(corners):\n", - " fig.add_trace(go.Scatter3d(\n", - " x=[x],\n", - " y=[y],\n", - " z=[z],\n", - " mode='markers+text',\n", - " text=[str(j)],\n", - " textposition='middle center',\n", - " marker=dict(size=5),\n", - " ))\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=str(plane_a.ids[i]),\n", + " name=str(plane_a.ids[i]),\n", + " marker=dict(size=1),\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", + "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(go.Scatter3d(\n", - " x=corners[:, 0],\n", - " y=corners[:, 1],\n", - " z=corners[:, 2],\n", - " mode='markers+lines',\n", - " name=str(plane_b.ids[i]),\n", - " marker=dict(size=1),\n", - " ))\n", - " for j, (x, y, z) in enumerate(corners):\n", - " fig.add_trace(go.Scatter3d(\n", - " x=[x],\n", - " y=[y],\n", - " z=[z],\n", - " mode='markers+text',\n", - " text=[str(j)],\n", - " 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", + " 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=str(plane_b.ids[i]),\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(go.Scatter3d(\n", - " x=corners[:, 0],\n", - " y=corners[:, 1],\n", - " z=corners[:, 2],\n", - " mode='markers+lines',\n", - " name=str(plane_c.ids[i]),\n", - " marker=dict(size=1),\n", - " ))\n", - " # corner id\n", - " for j, (x, y, z) in enumerate(corners):\n", - " fig.add_trace(go.Scatter3d(\n", - " x=[x],\n", - " y=[y],\n", - " z=[z],\n", - " mode='markers+text',\n", - " text=[str(j)],\n", - " 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", + " 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=str(plane_c.ids[i]),\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", + " name=\"normal_c\",\n", + " marker=dict(size=2),\n", + " )\n", + ")\n", "\n", "fig.update_layout(\n", " scene=dict(\n", - " aspectmode='cube',\n", + " aspectmode=\"cube\",\n", + " # yaxis_autorange=\"reversed\"\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", + " # reverse y\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", + "# yaxis=dict(range=[params.total_side_length, -0.1]),\n", "# zaxis=dict(range=[-0.1, params.total_side_length]),\n", "# )\n", "# )\n", diff --git a/new_try.ipynb b/new_try.ipynb new file mode 100644 index 0000000..47e5bf1 --- /dev/null +++ b/new_try.ipynb @@ -0,0 +1,2221 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 55, + "metadata": {}, + "outputs": [], + "source": [ + "from dataclasses import dataclass\n", + "import numpy as np\n", + "from matplotlib import pyplot as plt\n", + "import plotly.graph_objects as go\n", + "\n", + "NDArray = np.ndarray" + ] + }, + { + "cell_type": "code", + "execution_count": 56, + "metadata": {}, + "outputs": [], + "source": [ + "# 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", + "class ArUcoMarker2D:\n", + " id: int\n", + " _corners: NDArray\n", + "\n", + " def __init__(self, id: int, corners: Quad2D):\n", + " self.id = id\n", + " tmp = np.array(corners, dtype=np.float32)\n", + " assert tmp.shape == (4, 2)\n", + " self._corners = tmp\n", + "\n", + " @property\n", + " def corners(self):\n", + " return self._corners\n", + " \n", + " @property\n", + " def center(self):\n", + " return np.mean(self.corners, axis=0)\n", + "\n", + "\n", + "class ArUcoMarker3D:\n", + " id: int\n", + " _corners: NDArray\n", + "\n", + " def __init__(self, id: int, corners: NDArray):\n", + " self.id = id\n", + " tmp = np.array(corners, dtype=np.float32)\n", + " assert tmp.shape == (4, 3)\n", + " self._corners = tmp\n", + "\n", + " @staticmethod\n", + " def from_2d(marker2d: ArUcoMarker2D, z: float = 0.0):\n", + " return ArUcoMarker3D(\n", + " marker2d.id, np.column_stack((marker2d.corners, np.full((4,), z)))\n", + " )\n", + "\n", + " @property\n", + " def corners(self):\n", + " return self._corners\n", + "\n", + " @property\n", + " def center(self):\n", + " return np.mean(self.corners, axis=0)\n", + "\n", + " def normal(self, length: float = 1):\n", + " \"\"\"\n", + " return (2, 3)\n", + " \"\"\"\n", + " x, y, _ = self.center\n", + " return np.array([(x, y, 0), (x, y, length)])\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(\n", + " ids[0],\n", + " tl_to_square(tl_0_x, tl_0_y, params.marker_leghth),\n", + " ),\n", + " ArUcoMarker2D(\n", + " ids[1],\n", + " tl_to_square(tl_1_x, tl_1_y, params.marker_leghth),\n", + " ),\n", + " ArUcoMarker2D(\n", + " ids[2],\n", + " tl_to_square(tl_2_x, tl_2_y, params.marker_leghth),\n", + " ),\n", + " ArUcoMarker2D(\n", + " ids[3],\n", + " tl_to_square(tl_3_x, tl_3_y, params.marker_leghth),\n", + " ),\n", + " )" + ] + }, + { + "cell_type": "code", + "execution_count": 57, + "metadata": {}, + "outputs": [], + "source": [ + "params = DiamondBoardParameter(0.097, 0.127)\n", + "markers = generate_diamond_corners((16, 17, 18, 19), params)" + ] + }, + { + "cell_type": "code", + "execution_count": 60, + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "mode": "lines+markers", + "name": "Marker 16", + "type": "scatter", + "x": [ + 0.152, + 0.249, + 0.249, + 0.152, + 0.2005 + ], + "y": [ + 0.025, + 0.025, + 0.122, + 0.122, + 0.0735 + ] + }, + { + "mode": "lines+markers", + "name": "Marker 17", + "type": "scatter", + "x": [ + 0.025, + 0.122, + 0.122, + 0.025, + 0.0735 + ], + "y": [ + 0.152, + 0.152, + 0.249, + 0.249, + 0.2005 + ] + }, + { + "mode": "lines+markers", + "name": "Marker 18", + "type": "scatter", + "x": [ + 0.279, + 0.376, + 0.376, + 0.279, + 0.3275 + ], + "y": [ + 0.152, + 0.152, + 0.249, + 0.249, + 0.2005 + ] + }, + { + "mode": "lines+markers", + "name": "Marker 19", + "type": "scatter", + "x": [ + 0.152, + 0.249, + 0.249, + 0.152, + 0.2005 + ], + "y": [ + 0.279, + 0.279, + 0.376, + 0.376, + 0.3275 + ] + } + ], + "layout": { + "height": 600, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "heatmapgl": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmapgl" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "width": 600, + "yaxis": { + "autorange": "reversed" + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "fig = go.Figure()\n", + "for marker in markers:\n", + " corners = marker.corners\n", + " center = marker.center\n", + " corners = np.append(corners, [center], axis=0)\n", + " fig.add_trace(go.Scatter(x=corners[:, 0], y=corners[:, 1], mode='lines+markers', name=f\"Marker {marker.id}\"))\n", + "\n", + "# set the aspect ratio as 1:1\n", + "fig.update_layout(\n", + " width=600,\n", + " height=600,\n", + ")\n", + "fig.update_yaxes(autorange=\"reversed\")\n", + "fig.show()" + ] + }, + { + "cell_type": "code", + "execution_count": 61, + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "line": { + "width": 2 + }, + "marker": { + "size": 2 + }, + "mode": "lines+markers+text", + "name": "Marker 16", + "text": "16", + "textposition": "top center", + "type": "scatter3d", + "x": [ + 0.152, + 0.249, + 0.249, + 0.152 + ], + "y": [ + 0.025, + 0.025, + 0.122, + 0.122 + ], + "z": [ + 0, + 0, + 0, + 0 + ] + }, + { + "line": { + "width": 2 + }, + "mode": "lines", + "name": "Normal 16", + "type": "scatter3d", + "x": [ + 0.2004999965429306, + 0.2004999965429306 + ], + "y": [ + 0.07349999994039536, + 0.07349999994039536 + ], + "z": [ + 0, + 0.1 + ] + }, + { + "line": { + "width": 2 + }, + "marker": { + "size": 2 + }, + "mode": "lines+markers+text", + "name": "Marker 17", + "text": "17", + "textposition": "top center", + "type": "scatter3d", + "x": [ + 0.025, + 0.122, + 0.122, + 0.025 + ], + "y": [ + 0.152, + 0.152, + 0.249, + 0.249 + ], + "z": [ + 0, + 0, + 0, + 0 + ] + }, + { + "line": { + "width": 2 + }, + "mode": "lines", + "name": "Normal 17", + "type": "scatter3d", + "x": [ + 0.07349999994039536, + 0.07349999994039536 + ], + "y": [ + 0.2004999965429306, + 0.2004999965429306 + ], + "z": [ + 0, + 0.1 + ] + }, + { + "line": { + "width": 2 + }, + "marker": { + "size": 2 + }, + "mode": "lines+markers+text", + "name": "Marker 18", + "text": "18", + "textposition": "top center", + "type": "scatter3d", + "x": [ + 0.279, + 0.376, + 0.376, + 0.279 + ], + "y": [ + 0.152, + 0.152, + 0.249, + 0.249 + ], + "z": [ + 0, + 0, + 0, + 0 + ] + }, + { + "line": { + "width": 2 + }, + "mode": "lines", + "name": "Normal 18", + "type": "scatter3d", + "x": [ + 0.32749998569488525, + 0.32749998569488525 + ], + "y": [ + 0.2004999965429306, + 0.2004999965429306 + ], + "z": [ + 0, + 0.1 + ] + }, + { + "line": { + "width": 2 + }, + "marker": { + "size": 2 + }, + "mode": "lines+markers+text", + "name": "Marker 19", + "text": "19", + "textposition": "top center", + "type": "scatter3d", + "x": [ + 0.152, + 0.249, + 0.249, + 0.152 + ], + "y": [ + 0.279, + 0.279, + 0.376, + 0.376 + ], + "z": [ + 0, + 0, + 0, + 0 + ] + }, + { + "line": { + "width": 2 + }, + "mode": "lines", + "name": "Normal 19", + "type": "scatter3d", + "x": [ + 0.2004999965429306, + 0.2004999965429306 + ], + "y": [ + 0.32749998569488525, + 0.32749998569488525 + ], + "z": [ + 0, + 0.1 + ] + } + ], + "layout": { + "scene": { + "yaxis": { + "autorange": "reversed" + } + }, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "heatmapgl": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmapgl" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "markers_3d = [ArUcoMarker3D.from_2d(m) for m in markers]\n", + "fig = go.Figure()\n", + "for m in markers_3d:\n", + " fig.add_trace(\n", + " go.Scatter3d(\n", + " x=m.corners[:, 0],\n", + " y=m.corners[:, 1],\n", + " z=m.corners[:, 2],\n", + " mode=\"lines+markers+text\",\n", + " marker=dict(size=2),\n", + " line=dict(width=2),\n", + " textposition=\"top center\",\n", + " text=m.id,\n", + " name=f\"Marker {m.id}\",\n", + " )\n", + " )\n", + " n = m.normal(0.1)\n", + " fig.add_trace(\n", + " go.Scatter3d(\n", + " x=n[:, 0],\n", + " y=n[:, 1],\n", + " z=n[:, 2],\n", + " mode=\"lines\",\n", + " line=dict(width=2),\n", + " name=f\"Normal {m.id}\",\n", + " )\n", + " )\n", + "# note that the Y axis is reversed\n", + "fig.update_layout(scene=dict(yaxis_autorange=\"reversed\"))\n", + "fig.show()" + ] + } + ], + "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 +}