diff --git a/autopilot.py b/autopilot.py index 0a4c8ef..50f34c0 100644 --- a/autopilot.py +++ b/autopilot.py @@ -67,15 +67,13 @@ class AutoPilot(Pilot): # Инициализация ORB детектора self.orb_detector = cv2.ORB_create( - nfeatures=3000, - scaleFactor=1.2, - nlevels=8, - edgeThreshold=15, - firstLevel=0, - WTA_K=2, + nfeatures=300, + scaleFactor=1.3, + nlevels=4, + edgeThreshold=19, patchSize=31, - fastThreshold=8, - scoreType=cv2.ORB_HARRIS_SCORE + fastThreshold=15, + scoreType=cv2.ORB_FAST_SCORE # FAST обычно быстрее ) # Инициализация матчера для сопоставления ключевых точек (kNN + Lowe ratio) @@ -113,7 +111,7 @@ class AutoPilot(Pilot): if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4: return None, None, None, None, None - + # kNN сопоставление и тест Лоу matches_knn = self.bf_matcher.knnMatch(des1, des2, k=2) good_matches: list[cv2.DMatch] = [] @@ -196,7 +194,7 @@ class AutoPilot(Pilot): a21, a22 = H[1, 0], H[1, 1] # Смещение (уже отцентрировано) - tx, ty = RH[0, 2] * MOVE_KOF, RH[1, 2] * MOVE_KOF + tx, ty = -H[0, 2] * MOVE_KOF, -H[1, 2] * MOVE_KOF print(" [Pilot] translate:", tx, ty) @@ -317,54 +315,58 @@ class AutoPilot(Pilot): kp1, kp2, matches, transformation_info) - self.vis_manager.update_motion_vectors( - current_image, - kp1, kp2, matches) - mask = transformation_info['mask'] - self.vis_manager.update_motion_gomography( - current_image, + self.vis_manager.update_motion_vectors( + current_image, kp1, kp2, np.array(matches)[mask.ravel().astype(bool)]) + + # Используем новую визуализацию движения точек по сетке + homography_matrix = transformation_info['homography'] + self.vis_manager.update_homography_grid( + current_image, + homography_matrix, + 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: - # Оцениваем матрицу трансформации - 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 - self.x = landmark_world_x + dx_global - self.y = landmark_world_y + dy_global - self.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})") + # 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 + # self.x = landmark_world_x + dx_global + # self.y = landmark_world_y + dy_global + # self.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})") if distance_to_target < 75: self.target_idx += 1 @@ -372,6 +374,11 @@ class AutoPilot(Pilot): if self.target_idx == len(self.points): return PilotCommand(stop=True) + distance_to_target = math.sqrt( + (self.points[self.target_idx][0] - self.x) ** 2 + + (self.points[self.target_idx][1] - self.y) ** 2 + ) + # Вычисляем угол к цели target_angle = math.atan2(self.points[self.target_idx][1] - self.y, self.points[self.target_idx][0] - self.x) diff --git a/test_autopilot.ipynb b/test_autopilot.ipynb index 8a9dfe6..12e0d08 100644 --- a/test_autopilot.ipynb +++ b/test_autopilot.ipynb @@ -6,81 +6,11 @@ "metadata": {}, "outputs": [ { - "name": "stdout", + "name": "stderr", "output_type": "stream", "text": [ - "\n", - " [Pilot] translate: 0.05017116751607286 9.687502397756333\n", - " [Pilot] Drone Position: (9.63, 1.05)\n", - " [Pilot] Angle: 5.9°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1802.7756377319947\n", - " [Pilot] translate: 34.49815603085627 29.137861884347547\n", - "\n", - " [Pilot] translate: -3.880257616756106 51.463125720098006\n", - " [Pilot] Drone Position: (60.82, 7.61)\n", - " [Pilot] Angle: 11.6°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1794.1861357457751\n", - " [Pilot] translate: -81.10123958005454 -137.34212482489963\n", - "\n", - " [Pilot] translate: -4.965415715097187 51.25399608516978\n", - " [Pilot] Drone Position: (111.23, 18.11)\n", - " [Pilot] Angle: 17.3°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1748.165148948221\n", - " [Pilot] translate: 45.12386596220679 -80.30038654495203\n", - "\n", - " [Pilot] translate: -4.306576877574656 51.03219412293733\n", - " [Pilot] Drone Position: (159.88, 34.13)\n", - " [Pilot] Angle: 23.1°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1700.8189617850037\n", - " [Pilot] translate: 21.716104890355993 -140.57795924978092\n", - "\n", - " [Pilot] translate: -4.698241337005581 50.82696807577958\n", - " [Pilot] Drone Position: (206.68, 54.50)\n", - " [Pilot] Angle: 28.8°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1651.9184793917282\n", - " [Pilot] translate: -105.83895859495036 -240.1764870881905\n", - "\n", - " [Pilot] translate: -4.958766997526737 51.41923183115156\n", - " [Pilot] Drone Position: (251.86, 79.55)\n", - " [Pilot] Angle: 34.5°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1602.0752180361662\n", - " [Pilot] translate: -148.22727851217576 277.70124157634604\n", - "\n", - " [Pilot] translate: -3.092076978507596 51.453303771043075\n", - " [Pilot] Drone Position: (294.20, 108.95)\n", - " [Pilot] Angle: 38.2°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1550.834822541291\n", - " [Pilot] translate: 84.73919078536481 6.965084966855142\n", - "\n", - " [Pilot] translate: 0.6990361118758528 51.266860568260554\n", - " [Pilot] Drone Position: (333.90, 141.39)\n", - " [Pilot] Angle: 38.5°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1499.310186566892\n", - " [Pilot] translate: -169.29950811743842 109.96288683148794\n", - "\n", - " [Pilot] translate: 0.06894026606203722 51.71176082802062\n", - " [Pilot] Drone Position: (374.40, 173.54)\n", - " [Pilot] Angle: 38.4°\n", - " [Pilot] Target Index: 0\n", - " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1448.1010213418751\n", - " [Pilot] translate: -175.2651402920743 -178.56804299442655\n" + "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" ] } ], @@ -91,27 +21,81 @@ "from autopilot import AutoPilot\n", "from visualization import VisualizationManager\n", "\n", - "autopilot = AutoPilot([(1500, 1000)], [])\n", - "imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(10)]\n", - "autopilot.handle(imgs[0])\n", - "for i in range(1, 10):\n", - " print()\n", - " autopilot.handle(imgs[i])\n" + "vzm = VisualizationManager()\n", + "\n" ] }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "autopilot = AutoPilot([(0, 0), (1500, 1000)], [], vzm)\n", + "imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(10)]\n", + "autopilot.handle(imgs[0])\n" + ] + }, + { + "cell_type": "code", + "execution_count": 4, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "None\n" + "\n", + " [Pilot] translate: -0.28939364297583925 8.879763334079307\n", + " [Pilot] Drone Position: (8.87, 0.58)\n", + " [Pilot] Angle: 5.6°\n", + " [Pilot] Target Index: 0\n", + " [Pilot] Target Position: (0, 0)\n", + " [Pilot] Distance: 0.0\n", + "0.1 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] Target Index: 1\n", + " [Pilot] Target Position: (1500, 1000)\n", + " [Pilot] Distance: 1795.0821692652123\n", + "0.1 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] Target Index: 1\n", + " [Pilot] Target Position: (1500, 1000)\n", + " [Pilot] Distance: 1749.0606665752869\n", + "0.1 50.0\n" ] } ], + "source": [ + "for i in range(1, 4):\n", + " print()\n", + " command = autopilot.handle(imgs[i])\n", + " print(command.dangle, command.velocity)\n", + " vzm.pause(0.5)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], "source": [] } ], diff --git a/todo.md b/todo.md index eb2df9b..966e190 100644 --- a/todo.md +++ b/todo.md @@ -1,16 +1,18 @@ [+] График межкадрового смещения | [+] График межкадровых смещениях по версии матрицы гомографии | [+] Уменьшение погрешность за счёт удаления выбросов -| [-] Исследовать причину погрешности при развороте +| [+] Исследовать причину погрешности при развороте +[+] Устранение большой погрешности при повороте [-] Отображение bounding box на графике сопоставления точек кадра с ориентиром [-] Проверка корректности выявления ориентира на кадре [-] Исправление коррекции координат на основе сопоставления с ориентиром [-] FPS счетчик -[-] Устранение большой погрешности при повороте +| [-] Оптимизация детекции точек [-] Оформление статистики при тестовых запусках [-] Проведение тестовых запусков [-] Оформление отчета +[-] Эксперименты с разными детекторами (SIFT, KAZE) [?] Изменение масштаба во время полёта, обработка этой трансформации [?] Поворот ориентиров diff --git a/visualization.py b/visualization.py index c7ad133..d20450a 100644 --- a/visualization.py +++ b/visualization.py @@ -75,7 +75,7 @@ class VisualizationManager: self.ax_error_plot = self.fig.add_subplot(gs[0, 0]) self.ax_error_plot.set_title('Погрешность позиции от времени') self.ax_error_plot.set_xlabel('Время (кадры)') - self.ax_error_plot.set_ylabel('Погрешность (метры)') + self.ax_error_plot.set_ylabel('Погрешность (пиксели)') self.ax_error_plot.grid(True, alpha=0.3) # Глобальная карта (левый средний угол) @@ -384,6 +384,152 @@ class VisualizationManager: def update_motion_gomography(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None): self._update_motion_vectors(self.ax_motion_gomography, current_frame, prev_keypoints, current_keypoints, matches) + + def update_homography_grid(self, current_frame: np.ndarray, homography_matrix: np.ndarray, grid_step: int = 80): + """ + Визуализирует движение точек по сетке на основе матрицы гомографии + """ + self.ax_motion_gomography.clear() + self.ax_motion_gomography.set_title('Движение точек по сетке') + + if current_frame is None or homography_matrix is None: + self.ax_motion_gomography.axis('off') + return + + # Конвертируем BGR в RGB для matplotlib + if len(current_frame.shape) == 3 and current_frame.shape[2] == 3: + frame_rgb = cv2.cvtColor(current_frame, cv2.COLOR_BGR2RGB) + else: + frame_rgb = current_frame + + # Показываем текущий кадр + self.ax_motion_gomography.imshow(frame_rgb) + + # Получаем размеры изображения и центр + height, width = current_frame.shape[:2] + center_x, center_y = width // 2, height // 2 + + # Создаем сетку точек с заданным шагом + grid_points = [] + for y in range(grid_step, height, grid_step): + for x in range(grid_step, width, grid_step): + grid_points.append([x, y]) + + if len(grid_points) == 0: + self.ax_motion_gomography.axis('off') + return + + # Конвертируем в numpy массив и отцентрируем координаты относительно центра изображения + grid_points = np.array(grid_points, dtype=np.float32) + grid_points_centered = [] + for pt in grid_points: + # Отцентрируем координаты точно так же, как в detect_and_match_keypoints + centered_x = pt[0] - center_x + centered_y = center_y - pt[1] # Инвертируем Y (изображение Y направлен вниз) + grid_points_centered.append([centered_x, centered_y]) + + grid_points_centered = np.array(grid_points_centered, dtype=np.float32) + grid_points_homogeneous = np.column_stack([grid_points_centered, np.ones(len(grid_points_centered))]) + + # Применяем матрицу гомографии + transformed_points_homogeneous = homography_matrix @ grid_points_homogeneous.T + transformed_points_homogeneous = transformed_points_homogeneous.T + + # Нормализуем по третьей координате (перспективное преобразование) + transformed_points_centered = transformed_points_homogeneous[:, :2] / transformed_points_homogeneous[:, 2:3] + + # Конвертируем обратно в координаты изображения + transformed_points = [] + for pt in transformed_points_centered: + # Обратное преобразование от центрированных координат к координатам изображения + img_x = pt[0] + center_x + img_y = center_y - pt[1] # Инвертируем Y обратно + transformed_points.append([img_x, img_y]) + + transformed_points = np.array(transformed_points, dtype=np.float32) + + # Фильтруем точки, которые остались в пределах изображения + valid_indices = ( + (transformed_points[:, 0] >= 0) & + (transformed_points[:, 0] < width) & + (transformed_points[:, 1] >= 0) & + (transformed_points[:, 1] < height) + ) + + if np.sum(valid_indices) == 0: + self.ax_motion_gomography.axis('off') + return + + # Получаем валидные исходные и трансформированные точки + valid_source_points = grid_points[valid_indices] + valid_transformed_points = transformed_points[valid_indices] + + # Вычисляем векторы движения + motion_vectors = valid_transformed_points - valid_source_points + + # Вычисляем длину и направление векторов + vector_lengths = np.linalg.norm(motion_vectors, axis=1) + + if len(vector_lengths) > 0: + # Нормализуем длины для цветовой карты (0-1) + max_length = np.max(vector_lengths) + if max_length > 0: + normalized_lengths = vector_lengths / max_length + else: + normalized_lengths = np.zeros_like(vector_lengths) + + # Рисуем векторы движения + for i, (start_pt, end_pt, length, norm_length) in enumerate( + zip(valid_source_points, valid_transformed_points, vector_lengths, normalized_lengths)): + + # Пропускаем очень маленькие векторы + if length < 1.0: + continue + + # Цвет зависит от длины вектора (синий -> красный) + if norm_length < 0.5: + color = [0, norm_length * 2, 1 - norm_length * 2] # Синий -> Голубой + else: + color = [(norm_length - 0.5) * 2, 1 - (norm_length - 0.5) * 2, 0] # Голубой -> Красный + + # Толщина линии зависит от длины вектора + linewidth = max(1, min(4, 1 + 3 * norm_length)) + + # Рисуем вектор + self.ax_motion_gomography.arrow( + start_pt[0], start_pt[1], + end_pt[0] - start_pt[0], end_pt[1] - start_pt[1], + head_width=3, head_length=5, + fc=color, ec=color, alpha=0.8, linewidth=linewidth + ) + + # Рисуем исходную точку сетки + self.ax_motion_gomography.plot(start_pt[0], start_pt[1], 'o', + color='green', markersize=3, alpha=0.7) + + # Рисуем целевую точку + self.ax_motion_gomography.plot(end_pt[0], end_pt[1], 's', + color='red', markersize=2, alpha=0.7) + + # Добавляем информацию о статистике + avg_length = np.mean(vector_lengths) + max_length = np.max(vector_lengths) + total_points = len(vector_lengths) + + # info_text = f"Точек сетки: {total_points}\nСреднее движение: {avg_length:.1f}px\nМакс. движение: {max_length:.1f}px" + # self.ax_motion_gomography.text( + # 10, 30, info_text, fontsize=8, color='white', + # bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8) + # ) + + # Добавляем легенду + # legend_text = "Зеленые точки: исходные\nКрасные квадраты: целевые\nЦвет стрелок: скорость" + # self.ax_motion_gomography.text( + # 10, 90, legend_text, fontsize=7, color='white', + # bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8) + # ) + + self.ax_motion_gomography.axis('off') def update_display(self): """Обновляет отображение всех областей"""