zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add shared window

Browse Source
This commit is contained in:
russian_proger
2025-06-30 01:33:09 +03:00
parent 8b6bb3e22c
commit 2d85d78def
4 changed files with 316 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
autopilot.py
View File

@@ -19,13 +19,17 @@ class AutoPilot(Pilot):
orb_detector: cv2.ORB
bf_matcher: cv2.BFMatcher
frame_count: int
image_center: tuple # Центр изображения (x, y)
viz_manager: object # Менеджер визуализации (опционально)
def __init__(self, initial_x: float = 0.0, initial_y: float = 0.0):
def __init__(self, viz_manager=None):
self.prev_image = None
self.angle = 0.0
self.x = initial_x
self.y = initial_y
self.x = 0.0
self.y = 0.0
self.frame_count = 0
self.image_center = (0, 0) # Будет обновлено при получении первого изображения
self.viz_manager = viz_manager # Менеджер визуализации
# Инициализация ORB детектора
self.orb_detector = cv2.ORB_create(
@@ -72,15 +76,35 @@ class AutoPilot(Pilot):
if len(good_matches) < 4:
return None, None, None, None, None
# Извлечение координат сопоставленных точек
src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2)
# Получаем центр изображения
height1, width1 = img1.shape[:2]
height2, width2 = img2.shape[:2]
center_x1, center_y1 = width1 // 2, height1 // 2
center_x2, center_y2 = width2 // 2, height2 // 2
# Извлекаем координаты сопоставленных точек и отцентрируем их
src_pts = []
dst_pts = []
for match in good_matches:
# Координаты точки в первом изображении
pt1 = kp1[match.queryIdx].pt
src_pts.append([pt1[0] - center_x1, pt1[1] - center_y1])
# Координаты точки во втором изображении
pt2 = kp2[match.trainIdx].pt
dst_pts.append([pt2[0] - center_x2, pt2[1] - center_y2])
# Конвертируем в numpy массивы
src_pts = np.float32(src_pts).reshape(-1, 1, 2)
dst_pts = np.float32(dst_pts).reshape(-1, 1, 2)
return src_pts, dst_pts, good_matches, kp1, kp2
def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray):
"""
Оценивает матрицу трансформации на основе сопоставленных ключевых точек
Точки уже отцентрированы относительно центра изображения
"""
# Используем RANSAC для оценки матрицы гомографии
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
@@ -97,7 +121,7 @@ class AutoPilot(Pilot):
a11, a12 = H[0, 0], H[0, 1]
a21, a22 = H[1, 0], H[1, 1]
# Смещение
# Смещение (уже отцентрировано)
tx, ty = H[0, 2], H[1, 2]
# Вычисляем угол поворота
@@ -109,7 +133,7 @@ class AutoPilot(Pilot):
scale = (scale_x + scale_y) / 2
return {
'translation': (tx, ty),
'translation': (tx, ty), # Уже отцентрировано
'rotation': angle,
'scale': scale,
'homography': H,
@@ -119,12 +143,13 @@ class AutoPilot(Pilot):
def update_drone_position(self, transformation_info: dict):
"""
Обновляет позицию и угол БПЛА на основе трансформации изображения
Координаты уже отцентрированы относительно центра изображения
"""
tx, ty = transformation_info['translation']
rotation = transformation_info['rotation']
scale = transformation_info['scale']
# Конвертируем смещение в пикселях в метры
# Координаты уже отцентрированы, поэтому используем их напрямую
dx_meters = tx
dy_meters = ty
@@ -133,8 +158,9 @@ class AutoPilot(Pilot):
sin_angle = math.sin(self.angle)
# Поворачиваем смещение в глобальные координаты
dx_global = dx_meters * cos_angle - dy_meters * sin_angle
dy_global = dx_meters * sin_angle + dy_meters * cos_angle
# Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
dx_global = dx_meters * cos_angle - (-dy_meters) * sin_angle
dy_global = dx_meters * sin_angle + (-dy_meters) * cos_angle
# Обновляем координаты БПЛА
self.x += dx_global
@@ -176,18 +202,20 @@ class AutoPilot(Pilot):
info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
info_text3 = f"Scale: {scale:.2f}"
info_text4 = f"Image Center: {self.image_center}"
cv2.putText(img_matches, info_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.putText(img_matches, info_text2, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.putText(img_matches, info_text3, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.putText(img_matches, info_text4, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# Добавляем информацию о позиции БПЛА
drone_state = self.get_drone_state()
pos_text = f"Drone Pos: ({drone_state['x']:.2f}, {drone_state['y']:.2f})"
angle_text = f"Drone Angle: {drone_state['angle_degrees']:.1f}°"
cv2.putText(img_matches, pos_text, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
cv2.putText(img_matches, angle_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
cv2.putText(img_matches, pos_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
cv2.putText(img_matches, angle_text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
return img_matches
@@ -196,11 +224,24 @@ class AutoPilot(Pilot):
if self.prev_image is None:
self.prev_image = self.image_to_numpy(image)
# Вычисляем центр изображения
height, width = self.prev_image.shape[:2]
self.image_center = (width // 2, height // 2)
# Обновляем визуализацию детекции
if self.viz_manager:
kp, _ = self.orb_detector.detectAndCompute(self.prev_image, None)
self.viz_manager.update_detection(self.prev_image, kp)
return
# Конвертируем текущее изображение
current_image = self.image_to_numpy(image)
# Обновляем центр изображения
height, width = current_image.shape[:2]
self.image_center = (width // 2, height // 2)
# Обнаруживаем и сопоставляем ключевые точки
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(self.prev_image, current_image)
@@ -209,11 +250,6 @@ class AutoPilot(Pilot):
transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
if transformation_info:
# print(f"Frame {self.frame_count}:")
# print(f" Translation: {transformation_info['translation']}")
# print(f" Rotation: {transformation_info['rotation']:.4f} rad")
# print(f" Scale: {transformation_info['scale']:.4f}")
# Обновляем позицию и угол БПЛА
self.update_drone_position(transformation_info)
@@ -222,11 +258,20 @@ class AutoPilot(Pilot):
print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
# Визуализация (опционально)
img_matches = self.visualize_matches(self.prev_image, current_image,
# Обновляем визуализацию
if self.viz_manager:
# Обновляем детекцию ключевых точек
self.viz_manager.update_detection(current_image, kp2)
# Обновляем сопоставление точек
self.viz_manager.update_matches(self.prev_image, current_image,
kp1, kp2, matches, transformation_info)
cv2.imshow('Drone Tracking', img_matches)
cv2.waitKey(1)
# Визуализация (опционально, если нет менеджера визуализации)
if not self.viz_manager:
img_matches = self.visualize_matches(self.prev_image, current_image,
kp1, kp2, matches, transformation_info)
cv2.imshow('Drone Tracking', img_matches)
cv2.waitKey(1)
# Обновляем предыдущее изображение
self.prev_image = current_image