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

ref: test version

Browse Source
This commit is contained in:
russian_proger
2025-10-04 14:04:32 +03:00
parent 7610fc6f50
commit dc8c869bcf
7 changed files with 225 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
autopilot.py
View File

@@ -58,20 +58,28 @@ class AutoPilot(Pilot):
self.image_center = (0, 0) # Будет обновлено при получении первого изображения
self.vis_manager = viz_manager # Менеджер визуализации
# Пороговые значения качества сопоставления/гомографии
self.min_inliers: int = 12
self.min_inlier_ratio: float = 0.5
self.max_reproj_rmse: float = 3.0
self.min_scale: float = 0.7
self.max_scale: float = 1.4
# Инициализация ORB детектора
self.orb_detector = cv2.ORB_create(
nfeatures=1000,
nfeatures=3000,
scaleFactor=1.2,
nlevels=8,
edgeThreshold=31,
edgeThreshold=15,
firstLevel=0,
WTA_K=2,
patchSize=31,
fastThreshold=20
fastThreshold=8,
scoreType=cv2.ORB_HARRIS_SCORE
)
# Инициализация матчера для сопоставления ключевых точек
self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# Инициализация матчера для сопоставления ключевых точек (kNN + Lowe ratio)
self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
self.points = points
self.keypoints = keypoints
@@ -88,31 +96,44 @@ class AutoPilot(Pilot):
"""
Обнаруживает ключевые точки ORB и сопоставляет их между двумя изображениями
"""
# Обнаружение ключевых точек и дескрипторов
kp1, des1 = self.orb_detector.detectAndCompute(img1, None)
kp2, des2 = self.orb_detector.detectAndCompute(img2, None)
# Предобработка (повышение контраста и устойчивости)
def preprocess(gray_img: np.ndarray) -> np.ndarray:
clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8))
return clahe.apply(gray_img)
if des1 is None or des2 is None:
# В градации серого
g1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) if len(img1.shape) == 3 else img1
g2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) if len(img2.shape) == 3 else img2
g1 = preprocess(g1)
g2 = preprocess(g2)
# Обнаружение ключевых точек и дескрипторов
kp1, des1 = self.orb_detector.detectAndCompute(g1, None)
kp2, des2 = self.orb_detector.detectAndCompute(g2, None)
if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4:
return None, None, None, None, None
# Сопоставление ключевых точек
matches = self.bf_matcher.match(des1, des2)
# kNN сопоставление и тест Лоу
matches_knn = self.bf_matcher.knnMatch(des1, des2, k=2)
good_matches: list[cv2.DMatch] = []
for m_n in matches_knn:
if len(m_n) < 2:
continue
m, n = m_n
if m.distance < 0.75 * n.distance:
good_matches.append(m)
# Сортировка совпадений по расстоянию
matches = sorted(matches, key=lambda x: x.distance)
# Фильтрация хороших совпадений (расстояние меньше порога)
good_matches = []
for match in matches:
if match.distance < 50: # Порог расстояния
good_matches.append(match)
# Дополнительная фильтрация по расстоянию (мягкий порог)
good_matches = sorted(good_matches, key=lambda x: x.distance)
good_matches = [m for m in good_matches if m.distance < 64]
if len(good_matches) < 4:
return None, None, None, None, None
# Получаем центр изображения
height1, width1 = img1.shape[:2]
height2, width2 = img2.shape[:2]
height1, width1 = g1.shape[:2]
height2, width2 = g2.shape[:2]
center_x1, center_y1 = width1 // 2, height1 // 2
center_x2, center_y2 = width2 // 2, height2 // 2
@@ -147,6 +168,24 @@ class AutoPilot(Pilot):
if H is None:
return None
# Маска инлайеров
inliers = int(mask.sum()) if mask is not None else 0
total = int(mask.size) if mask is not None else 0
inlier_ratio = (inliers / total) if total > 0 else 0.0
# Ошибка репроекции по инлайерам (RMSE)
rmse = None
try:
# Проецируем src через H и сравниваем с dst
proj = cv2.perspectiveTransform(src_pts, H)
errs = np.linalg.norm((proj - dst_pts).reshape(-1, 2), axis=1)
if mask is not None and mask.shape[0] == errs.shape[0]:
errs = errs[mask.ravel().astype(bool)]
if errs.size > 0:
rmse = float(np.sqrt(np.mean(errs ** 2)))
except Exception:
rmse = None
# Извлекаем параметры трансформации из матрицы гомографии
# H = [a11 a12 tx]
# [a21 a22 ty]
@@ -174,7 +213,10 @@ class AutoPilot(Pilot):
'rotation': angle,
'scale': scale,
'homography': H,
'mask': mask
'mask': mask,
'inliers': inliers,
'inliers_ratio': inlier_ratio,
'rmse': rmse
}
def update_drone_position(self, transformation_info: dict):
@@ -189,6 +231,9 @@ class AutoPilot(Pilot):
# Координаты уже отцентрированы, поэтому используем их напрямую
dx_meters = ty
dy_meters = tx
# Обновляем угол БПЛА
self.angle += rotation
# Применяем поворот к смещению (учитываем текущий угол БПЛА)
cos_angle = math.cos(self.angle)
@@ -203,9 +248,6 @@ class AutoPilot(Pilot):
self.x += dx_global
self.y += dy_global
# Обновляем угол БПЛА
self.angle += rotation
# Нормализуем угол в диапазоне [-π, π]
self.angle = math.atan2(math.sin(self.angle), math.cos(self.angle))
@@ -221,41 +263,6 @@ class AutoPilot(Pilot):
'frame_count': self.frame_count
}
def visualize_matches(self, img1: np.ndarray, img2: np.ndarray,
kp1, kp2, matches, transformation_info):
"""
Визуализирует сопоставленные ключевые точки и трансформацию
"""
# Рисуем сопоставления
img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None,
flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
# Добавляем информацию о трансформации
if transformation_info:
tx, ty = transformation_info['translation']
angle = transformation_info['rotation']
scale = transformation_info['scale']
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, 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
def handle(self, image: Image) -> PilotCommand:
self.frame_count += 1
@@ -294,13 +301,11 @@ class AutoPilot(Pilot):
transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
if transformation_info:
# Обновляем позицию и угол БПЛА
# Обновляем позицию и угол БПЛА (одометрия по кадрам)
self.update_drone_position(transformation_info)
# Выводим текущее состояние БПЛА
drone_state = self.get_drone_state()
if self.vis_manager:
self.vis_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
print(f" [Pilot] Target Index: {self.target_idx}")
@@ -322,13 +327,43 @@ class AutoPilot(Pilot):
# Пытаемся найти ориентир на картинке:
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:
# # Оцениваем матрицу трансформации
# transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
# if self.vis_manager:
# self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches, transformation_info)
if distance_to_target < 50:
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 distance_to_target < 75:
self.target_idx += 1
if self.target_idx == len(self.points):