diff --git a/autopilot.py b/autopilot.py index 50f34c0..07d7313 100644 --- a/autopilot.py +++ b/autopilot.py @@ -27,8 +27,6 @@ class PilotCommand: self.stop = stop self.velocity = velocity -MOVE_KOF: float = 1 / 1.2 - class AutoPilot(Pilot): # Состояние одометрии @@ -194,7 +192,7 @@ class AutoPilot(Pilot): a21, a22 = H[1, 0], H[1, 1] # Смещение (уже отцентрировано) - tx, ty = -H[0, 2] * MOVE_KOF, -H[1, 2] * MOVE_KOF + tx, ty = -H[0, 2], -H[1, 2] print(" [Pilot] translate:", tx, ty) @@ -260,6 +258,50 @@ class AutoPilot(Pilot): 'angle_degrees': math.degrees(self.angle), 'frame_count': self.frame_count } + + def get_position_by_chunk(self) -> tuple[float, float, float] | None: + # Пытаемся найти ориентир на картинке: + current_image = self.prev_image + landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB) + src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image) + + if src_pts is not None and dst_pts is not None and self.vis_manager: + self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches) + + if src_pts is not None and dst_pts is not None: + # Оцениваем матрицу трансформации + landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts) + # Если ориентир достоверно найден — скорректируем глобальные координаты и угол + if landmark_transform is not None: + ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale) + ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers) + ratio = landmark_transform.get('inliers_ratio', 0.0) + ok_ratio = (ratio >= self.min_inlier_ratio) + rmse = landmark_transform.get('rmse', None) + ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse) + if ok_scale and ok_inliers and ok_ratio and ok_rmse: + print("[PILOT]", rmse, ratio, ok_rmse) + # if False: + # Считаем абсолютную позу относительно координат ориентира + landmark_world_x, landmark_world_y = self.points[self.target_idx] + tx, ty = landmark_transform['translation'] + # Смещение в системе БПЛА: (dx_meters, dy_meters) + dx_meters = ty + dy_meters = tx + # Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх) + absolute_angle = landmark_transform['rotation'] + cos_a = math.cos(absolute_angle) + sin_a = math.sin(absolute_angle) + # Переход в глобальные координаты карты + dx_global = dx_meters * cos_a - dy_meters * sin_a + dy_global = dx_meters * sin_a + dy_meters * cos_a + x = landmark_world_x + dx_global + y = landmark_world_y + dy_global + angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle)) + print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})") + return (x, y, angle) + return None + def handle(self, image: Image) -> PilotCommand: self.frame_count += 1 @@ -330,11 +372,11 @@ class AutoPilot(Pilot): grid_step=85) # Пытаемся найти ориентир на картинке: - # landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB) - # src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image) + landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB) + src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image) - # if src_pts is not None and dst_pts is not None and self.vis_manager: - # self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches) + if src_pts is not None and dst_pts is not None and self.vis_manager: + self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches) # if src_pts is not None and dst_pts is not None: # # Оцениваем матрицу трансформации @@ -391,7 +433,7 @@ class AutoPilot(Pilot): angle_diff -= 2 * math.pi self.prev_image = current_image - return PilotCommand(max(min(0.1, angle_diff), -0.1), min(50., distance_to_target / 2)) + return PilotCommand(max(min(0.6, angle_diff), -0.6), min(50., distance_to_target / 2)) def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0): """Сбрасывает позицию и угол БПЛА""" diff --git a/main.py b/main.py index 6a646b1..ae31dfa 100644 --- a/main.py +++ b/main.py @@ -13,7 +13,7 @@ import autopilot def make_global_photo(filename): yandexMap = YandexMap() - yandexMap.save_photo() + yandexMap.save_photo(filename) yandexMap.destroy() def get_trajectory_points(bg_img: str) -> list[(float, float)]: @@ -28,28 +28,28 @@ def main(): # make_global_photo('map.jpg') # Получаем траекторию от пользователя - # points = get_trajectory_points('map.jpg') - # print(points) + points = get_trajectory_points('map.jpg') + print(points) # points = [np.float64(0.5443937502226799), np.float64(0.4030424838774785)], [np.float64(0.18517133490120316), np.float64(0.4586935604608052)], [np.float64(0.1641887171838272), np.float64(0.5586383510594329)], [np.float64(0.587198290366127), # np.float64(0.5699957136274587)] - points = [[np.float64(0.481445897070552), np.float64(0.49958006570569835)], [np.float64(0.5485902737661552), np.float64(0.4325716265543457)], [np.float64(0.5376793125531197), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.3689703961734008)], [np.float64(0.6333600493443542), np.float64(0.3224052096444948)], [np.float64(0.6686108471095458), np.float64(0.5518239335186174)]] + # points = [[np.float64(0.4931961629922826), np.float64(0.4439289891223716)], [np.float64(0.5099822571661834), np.float64(0.44165751660876645)], [np.float64(0.5083036477487933), np.float64(0.4564220879472001)], [np.float64(0.525089741922694), np.float64(0.4586935604608052)]] # Для каждой точки сделаем приближенный снимок yandexMap = YandexMap() keypoints: list[(any, any)] = [] - # plt.ion() - # for i in range(len(points)): - # point = points[i] - # yandexMap.scroll(point[0], point[1], 5, True) - # sleep(1) - # img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2) - # Path('chunks').mkdir(exist_ok=True) - # cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img) - # plt.subplot(1, len(points), i+1) - # plt.imshow(img) - # plt.pause(0.25) - # yandexMap.scroll(point[0], point[1], 5, False) + plt.ion() + for i in range(len(points)): + point = points[i] + yandexMap.scroll(point[0], point[1], 5, True) + sleep(1) + img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2) + Path('chunks').mkdir(exist_ok=True) + cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img) + plt.subplot(1, len(points), i+1) + plt.imshow(img) + plt.pause(0.25) + yandexMap.scroll(point[0], point[1], 5, False) plt.tight_layout() @@ -87,6 +87,7 @@ def main(): vis_manager = VisualizationManager() width, height = yandexMap.get_size() + print(width, height) points_coords = np.array(list(map(lambda p: [ (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height ], points))) diff --git a/map.jpg b/map.jpg index 3a18e86..24f1bec 100644 Binary files a/map.jpg and b/map.jpg differ diff --git a/test_autopilot.ipynb b/test_autopilot.ipynb index 12e0d08..8148a09 100644 --- a/test_autopilot.ipynb +++ b/test_autopilot.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 2, + "execution_count": 1, "metadata": {}, "outputs": [ { @@ -27,29 +27,27 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ - "autopilot = AutoPilot([(0, 0), (1500, 1000)], [], vzm)\n", + "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(10)]\n", - "autopilot.handle(imgs[0])\n" + "autopilot.handle(imgs[0])\n", + "vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n" ] }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 3, "metadata": {}, "outputs": [ { @@ -57,38 +55,104 @@ "output_type": "stream", "text": [ "\n", - " [Pilot] translate: -0.28939364297583925 8.879763334079307\n", - " [Pilot] Drone Position: (8.87, 0.58)\n", - " [Pilot] Angle: 5.6°\n", + " [Pilot] translate: -1.5380446518826545 49.21369409309772\n", + " [Pilot] Drone Position: (47.97, -11.12)\n", + " [Pilot] Angle: -11.3°\n", " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (0, 0)\n", + " [Pilot] Target Position: [0. 0.]\n", " [Pilot] Distance: 0.0\n", - "0.1 50.0\n", + "0.004129499336505804 50.0\n", "\n", - " [Pilot] translate: -4.194654791266826 51.7432078133843\n", - " [Pilot] Drone Position: (60.43, 6.60)\n", - " [Pilot] Angle: 11.3°\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: (1500, 1000)\n", - " [Pilot] Distance: 1795.0821692652123\n", - "0.1 50.0\n", + " [Pilot] Target Position: [347.32359333 -69.46471867]\n", + " [Pilot] Distance: 304.9907527982671\n", + "0.003525142621744054 50.0\n", "\n", - " [Pilot] translate: -4.752188299516379 51.16691704857423\n", - " [Pilot] Drone Position: (110.73, 17.12)\n", - " [Pilot] Angle: 17.1°\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: (1500, 1000)\n", - " [Pilot] Distance: 1749.0606665752869\n", - "0.1 50.0\n" + " [Pilot] Target Position: [347.32359333 -69.46471867]\n", + " [Pilot] Distance: 255.13708614259195\n", + "0.0019957432639431116 50.0\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.0008527889497844943 50.0\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.0006948810462804877 50.0\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", + "\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.951716943628604\n", + "-0.6 50.0\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.1393363134888\n", + "-0.6 50.0\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.07341344997275\n", + "-0.6 50.0\n", + "\n" + ] + }, + { + "ename": "IndexError", + "evalue": "list index out of range", + "output_type": "error", + "traceback": [ + "\u001b[31m---------------------------------------------------------------------------\u001b[39m", + "\u001b[31mIndexError\u001b[39m Traceback (most recent call last)", + "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[3]\u001b[39m\u001b[32m, line 3\u001b[39m\n\u001b[32m 1\u001b[39m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(\u001b[32m1\u001b[39m, \u001b[32m20\u001b[39m):\n\u001b[32m 2\u001b[39m \u001b[38;5;28mprint\u001b[39m()\n\u001b[32m----> \u001b[39m\u001b[32m3\u001b[39m command = autopilot.handle(\u001b[43mimgs\u001b[49m\u001b[43m[\u001b[49m\u001b[43mi\u001b[49m\u001b[43m]\u001b[49m)\n\u001b[32m 4\u001b[39m \u001b[38;5;28mprint\u001b[39m(command.dangle, command.velocity)\n\u001b[32m 5\u001b[39m vzm.set_target_index(autopilot.target_idx)\n", + "\u001b[31mIndexError\u001b[39m: list index out of range" ] } ], "source": [ - "for i in range(1, 4):\n", + "for i in range(1, 20):\n", " print()\n", " command = autopilot.handle(imgs[i])\n", " print(command.dangle, command.velocity)\n", - " vzm.pause(0.5)\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", + " if pos is not None:\n", + " print(pos)\n", + " vzm.update_global_map(pos[0], pos[1])\n", + " vzm.pause(2.5)\n" ] }, { diff --git a/todo.md b/todo.md index 966e190..1524770 100644 --- a/todo.md +++ b/todo.md @@ -1,12 +1,7 @@ -[+] График межкадрового смещения -| [+] График межкадровых смещениях по версии матрицы гомографии -| [+] Уменьшение погрешность за счёт удаления выбросов -| [+] Исследовать причину погрешности при развороте -[+] Устранение большой погрешности при повороте +[!] Отображение bounding box на графике сопоставления точек кадра с ориентиром +[!] Проверка корректности выявления ориентира на кадре +[!] Исправление коррекции координат на основе сопоставления с ориентиром -[-] Отображение bounding box на графике сопоставления точек кадра с ориентиром -[-] Проверка корректности выявления ориентира на кадре -[-] Исправление коррекции координат на основе сопоставления с ориентиром [-] FPS счетчик | [-] Оптимизация детекции точек [-] Оформление статистики при тестовых запусках @@ -17,3 +12,9 @@ [?] Изменение масштаба во время полёта, обработка этой трансформации [?] Поворот ориентиров [?] Ограничение выбора точек при построении маршрута, чтобы ориентиры полностью попадали в кадр + +[+] График межкадрового смещения +| [+] График межкадровых смещениях по версии матрицы гомографии +| [+] Уменьшение погрешность за счёт удаления выбросов +| [+] Исследовать причину погрешности при развороте +[+] Устранение большой погрешности при повороте diff --git a/yandex_map.py b/yandex_map.py index 3b5a913..815ea51 100644 --- a/yandex_map.py +++ b/yandex_map.py @@ -21,6 +21,7 @@ class YandexMap: # options.add_experimental_option("detach", True) self.driver = webdriver.Chrome(options) self.driver.get(generateURL()) + self.driver.maximize_window() sleep(2) action = ActionChains(self.driver)