316 lines
9.6 KiB
Plaintext
316 lines
9.6 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"c:\\Users\\admin\\Projects\\autopilot\\visualization.py:113: UserWarning: This figure includes Axes that are not compatible with tight_layout, so results might be incorrect.\n",
|
|
" plt.tight_layout()\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"from pathlib import Path\n",
|
|
"from PIL import Image\n",
|
|
"import numpy as np\n",
|
|
"from autopilot import AutoPilot\n",
|
|
"from visualization import VisualizationManager\n",
|
|
"\n",
|
|
"vzm = VisualizationManager()\n",
|
|
"\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"points = [[np.float64(0.48700765111972333), np.float64(0.49793590752304234)], [np.float64(0.4983137576734965), np.float64(0.5021469113219258)], [np.float64(0.4966178416904305), np.float64(0.5189909265174597)], [np.float64(0.4813545978428368), np.float64(0.5105689189196927)]]\n",
|
|
"width, height = 1920, 1031\n",
|
|
"points_coords = np.array(list(map(lambda p: [\n",
|
|
" (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height\n",
|
|
" ], points)))\n",
|
|
"points_coords *= 2 ** 4\n",
|
|
"\n",
|
|
"vzm.set_target_points(points_coords)\n",
|
|
"autopilot = AutoPilot(points_coords, [], vzm)\n",
|
|
"imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(30)]\n",
|
|
"autopilot.handle(imgs[0])\n",
|
|
"vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"-84.81833939223279\n",
|
|
"[[[ 0.43529349 -148.93070048]]]\n",
|
|
"164.0668026014628 -13.040291124133018 90.0\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"import cv2\n",
|
|
"import math\n",
|
|
"mat = np.array([[ 9.03878625e-02, 9.95998304e-01, -1.36910620e+01],\n",
|
|
" [-9.96729952e-01, 9.12780821e-02, 6.55573605e+00],\n",
|
|
" [ 1.28059260e-05, -4.64443066e-06, 1.00000000e+00]])\n",
|
|
"\n",
|
|
"x = 156.295617\n",
|
|
"y = 0\n",
|
|
"\n",
|
|
"print(np.atan2(mat[1, 0], mat[0, 0]) / math.pi * 180)\n",
|
|
"\n",
|
|
"print(cv2.perspectiveTransform(np.array([x, y]).reshape((1, 1, 2)), mat))\n",
|
|
"\n",
|
|
"nx, ny, na = autopilot.calc_position(np.linalg.inv(mat), x, y, math.pi / 2)\n",
|
|
"\n",
|
|
"print(nx, ny, na / math.pi * 180)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"\n",
|
|
" [Pilot] translate: -2.2243520368114386e-14 2.083887107525814e-15\n",
|
|
" [Pilot] Drone Position: (47.97, -11.12)\n",
|
|
" [Pilot] Angle: -11.3°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 304.9907527982671\n",
|
|
"0.004129499336505887 50.0\n",
|
|
"47.96512465732409 -11.122053958129657\n",
|
|
"\n",
|
|
" [Pilot] translate: -0.773373251191613 49.85522586610471\n",
|
|
" [Pilot] Drone Position: (96.75, -21.44)\n",
|
|
" [Pilot] Angle: -11.1°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 304.9907527982671\n",
|
|
"0.003525142621744165 50.0\n",
|
|
"96.7474713468631 -21.438269533988773\n",
|
|
"\n",
|
|
" [Pilot] translate: -0.4238729464016157 49.716078726704744\n",
|
|
" [Pilot] Drone Position: (145.50, -31.21)\n",
|
|
" [Pilot] Angle: -10.8°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 255.13708614259195\n",
|
|
"0.0019957432639432504 50.0\n",
|
|
"145.49554314093962 -31.210354705238323\n",
|
|
"\n",
|
|
" [Pilot] translate: -0.5072011337859063 49.476309464169745\n",
|
|
" [Pilot] Drone Position: (194.03, -40.83)\n",
|
|
" [Pilot] Angle: -10.6°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 205.42141613269357\n",
|
|
"0.0008527889497846608 50.0\n",
|
|
"194.02953935562192 -40.83395396007064\n",
|
|
"\n",
|
|
" [Pilot] translate: 0.11201474583194077 50.52107985317721\n",
|
|
" [Pilot] Drone Position: (243.70, -50.06)\n",
|
|
" [Pilot] Angle: -10.6°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 155.9448225243591\n",
|
|
"0.0006948810462807098 50.0\n",
|
|
"243.70149729872674 -50.058304936308375\n",
|
|
"\n",
|
|
" [Pilot] translate: 0.0398933070509686 50.472489376060885\n",
|
|
" [Pilot] Drone Position: (293.34, -59.18)\n",
|
|
" [Pilot] Angle: -10.5°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 105.42365806674246\n",
|
|
"-0.005757684837339738 50.0\n",
|
|
"293.3429112368944 -59.17991649962423\n",
|
|
"\n",
|
|
" [Pilot] translate: -0.5779730654942618 49.87019784134602\n",
|
|
" [Pilot] Drone Position: (342.21, -69.12)\n",
|
|
" [Pilot] Angle: -10.8°\n",
|
|
" [Pilot] Target Index: 1\n",
|
|
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
|
|
" [Pilot] Distance: 54.95171694362866\n",
|
|
"-0.6 50.0\n",
|
|
"342.21488384208834 -69.12481364723727\n",
|
|
"\n",
|
|
" [Pilot] translate: -0.3671006590822984 49.56271477490151\n",
|
|
" [Pilot] Drone Position: (376.83, -104.60)\n",
|
|
" [Pilot] Angle: -45.3°\n",
|
|
" [Pilot] Target Index: 2\n",
|
|
" [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
|
|
" [Pilot] Distance: 282.13933631348874\n",
|
|
"-0.6 50.0\n",
|
|
" [Pilot] translate: 152.48531037725778 -11.363336940584135\n",
|
|
"376.82813934296445 -104.60043911576437\n",
|
|
"\n",
|
|
" [Pilot] translate: 0.814116178363097 49.83161557706345\n",
|
|
" [Pilot] Drone Position: (386.60, -153.47)\n",
|
|
" [Pilot] Angle: -79.6°\n",
|
|
" [Pilot] Target Index: 2\n",
|
|
" [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
|
|
" [Pilot] Distance: 256.0734134499727\n",
|
|
"-0.6 50.0\n",
|
|
" [Pilot] translate: 223.2717832052611 -50.70818702916637\n",
|
|
"386.60059278853873 -153.47120593833623\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"for i in range(1, 10):\n",
|
|
" print()\n",
|
|
" command = autopilot.handle(imgs[i])\n",
|
|
" print(command.dangle, command.velocity)\n",
|
|
" vzm.set_target_index(autopilot.target_idx)\n",
|
|
" vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n",
|
|
" pos = autopilot.get_position_by_chunk()\n",
|
|
" print(autopilot.x, autopilot.y)\n",
|
|
" vzm.update_global_map(0, 0)\n",
|
|
" vzm.pause(2.5)\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 96,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"[[[ 0.18926711 -21.79121458]]]\n",
|
|
"0.40152777578126847\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"mat = np.array([[ 9.18211825e-01, -3.88263543e-01, 4.35846954e-01],\n",
|
|
" [ 3.89868385e-01, 9.19587423e-01, -2.49399645e+01],\n",
|
|
" [-3.56258907e-06, 3.01183248e-06, 1.00000000e+00]])\n",
|
|
"\n",
|
|
"pos = np.array([1., 3.]).reshape((1, 1, 2))\n",
|
|
"print(cv2.perspectiveTransform(pos, mat))\n",
|
|
"\n",
|
|
"(mat) @ np.array([1., 3., 1.]).reshape((3))\n",
|
|
"print(np.atan2(mat[1, 0], mat[0, 0]))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 95,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"[[-0.70710678 2.12132034]\n",
|
|
" [ 0.70710678 3.53553391]\n",
|
|
" [-2.12132034 3.53553391]\n",
|
|
" [ 0. 0. ]]\n",
|
|
"[[ 7.07106671e-01 -7.07106696e-01 -9.13243653e-17]\n",
|
|
" [ 7.07106528e-01 7.07106788e-01 3.65297461e-16]\n",
|
|
" [-7.16496367e-08 2.10734232e-09 1.00000000e+00]]\n",
|
|
"0.7853980622449934\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"ang = np.radians(45)\n",
|
|
"rot = np.array([\n",
|
|
" [np.cos(ang), -np.sin(ang)],\n",
|
|
" [np.sin(ang), np.cos(ang)]\n",
|
|
"])\n",
|
|
"\n",
|
|
"pts = np.array([\n",
|
|
" [1., 2.],\n",
|
|
" [3., 2.],\n",
|
|
" [1., 4.],\n",
|
|
" [0., 0.]\n",
|
|
"])\n",
|
|
"\n",
|
|
"dst = pts @ rot.T\n",
|
|
"\n",
|
|
"mat, mask = cv2.findHomography(pts, pts @ rot.T)\n",
|
|
"# print(np.atan2(mat[0, 1], mat[0, 0]))\n",
|
|
"# pts @ rot.T\n",
|
|
"pts @ mat[0:2]\n",
|
|
"# print(cv2.perspectiveTransform(pts[0].reshape((-1, 1, 2)), mat))\n",
|
|
"print(dst)\n",
|
|
"print(mat)\n",
|
|
"\n",
|
|
"\n",
|
|
"mat[0:2, 0:2] @ pts.T\n",
|
|
"\n",
|
|
"print(np.atan2(mat[1, 0], mat[0, 0]))\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": []
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 98,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"(1, 2, 3)\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"def a(*args):\n",
|
|
" print(args)\n",
|
|
"\n",
|
|
"a(*(1, 2, 3))"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": ".venv",
|
|
"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.11.0"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 2
|
|
}
|