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

fix: change coordinate system of simulator and autopilot, reduce approximation error

Browse Source
This commit is contained in:
russian_proger
2025-10-07 18:25:09 +03:00
parent 520a333812
commit f0ac60c8ef
5 changed files with 272 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

153
autopilot.py
View File

@@ -55,6 +55,7 @@ class AutoPilot(Pilot):
self.frame_count = 0
self.image_center = (0, 0) # Будет обновлено при получении первого изображения
self.vis_manager = viz_manager # Менеджер визуализации
self.reserved_pos = None
# Пороговые значения качества сопоставления/гомографии
self.min_inliers: int = 12
@@ -65,7 +66,7 @@ class AutoPilot(Pilot):
# Инициализация ORB детектора
self.orb_detector = cv2.ORB_create(
nfeatures=300,
nfeatures=1800,
scaleFactor=1.3,
nlevels=4,
edgeThreshold=19,
@@ -159,7 +160,7 @@ class AutoPilot(Pilot):
"""
# Используем RANSAC для оценки матрицы гомографии
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0)
RH, rmask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 1.0)
# RH, rmask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 1.0)
if H is None:
return None
@@ -214,26 +215,34 @@ class AutoPilot(Pilot):
'inliers_ratio': inlier_ratio,
'rmse': rmse
}
def calc_position(
self,
mat: np.ndarray,
x_source: float,
y_source: float,
angle_source: float
) -> tuple[float, float, float] | None:
""" Возвращает новые координаты и поворот на основе матрицы преобразования """
def update_drone_position(self, transformation_info: dict):
"""
Обновляет позицию и угол БПЛА на основе трансформации изображения
Координаты уже отцентрированы относительно центра изображения
"""
tx, ty = transformation_info['translation']
rotation = transformation_info['rotation']
scale = transformation_info['scale']
tx, ty = -mat[0, 2], -mat[1, 2]
# Вычисляем угол поворота
rotation = -np.arctan2(mat[1, 0], mat[0, 0])
print("[HOMOGRAPHY]", mat)
print("[ROTATION]", rotation)
# Координаты уже отцентрированы, поэтому используем их напрямую
dx_meters = ty
dy_meters = tx
dx_meters = tx
dy_meters = ty
# Обновляем угол БПЛА
self.angle += rotation
angle_global = angle_source + rotation
# Применяем поворот к смещению (учитываем текущий угол БПЛА)
cos_angle = math.cos(self.angle)
sin_angle = math.sin(self.angle)
cos_angle = math.cos(angle_global)
sin_angle = math.sin(angle_global)
# Поворачиваем смещение в глобальные координаты
# Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
@@ -241,11 +250,30 @@ class AutoPilot(Pilot):
dy_global = dx_meters * sin_angle + dy_meters * cos_angle
# Обновляем координаты БПЛА
self.x += dx_global
self.y += dy_global
x = x_source + dx_global
y = y_source + dy_global
# Нормализуем угол в диапазоне [-π, π]
self.angle = math.atan2(math.sin(self.angle), math.cos(self.angle))
angle = math.atan2(math.sin(angle_global), math.cos(angle_global))
return x, y, angle
def update_drone_position(self, transformation_info: dict):
"""
Обновляет позицию и угол БПЛА на основе трансформации изображения
Координаты уже отцентрированы относительно центра изображения
"""
homography = transformation_info['homography']
self.x, self.y, self.angle = self.calc_position(
homography,
self.x, self.y, self.angle
)
if self.reserved_pos is not None:
self.reserved_pos = self.calc_position(
homography, *self.reserved_pos
)
def get_drone_state(self) -> dict:
"""
@@ -258,15 +286,17 @@ 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)
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(landmark_image, current_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)
self.vis_manager.update_chunk_matches(landmark_image, current_image, kp1, kp2, matches)
if src_pts is not None and dst_pts is not None:
# Оцениваем матрицу трансформации
@@ -280,26 +310,19 @@ class AutoPilot(Pilot):
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("[HELP]")
print("Matrix", landmark_transform['homography'])
print("Position", self.x, self.y)
print("Position of point", self.points[self.target_idx])
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))
landmark_angle = 0
homography = landmark_transform['homography']
# homography = np.linalg.inv(homography)
print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
return (x, y, angle)
return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle)
return None
@@ -372,45 +395,12 @@ 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)
self.prev_image = current_image
npos = self.get_position_by_chunk()
if npos is not None:
self.reserved_pos = npos
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:
if distance_to_target < 35:
self.target_idx += 1
if self.target_idx == len(self.points):
@@ -421,19 +411,28 @@ class AutoPilot(Pilot):
(self.points[self.target_idx][1] - self.y) ** 2
)
if self.reserved_pos is not None:
self.x, self.y, self.angle = self.reserved_pos
self.reserved_pos = None
# Вычисляем угол к цели
target_angle = math.atan2(self.points[self.target_idx][1] - self.y, self.points[self.target_idx][0] - self.x)
angle_trajectory = self.angle + math.pi / 2
print("[ANGLE]", angle_trajectory, "->", target_angle)
# Вычисляем разность углов (направление поворота)
angle_diff = target_angle - self.angle
angle_diff = target_angle - angle_trajectory
# Нормализуем разность углов в диапазон [-π, π]
angle_diff %= 2 * math.pi
if angle_diff >= math.pi:
angle_diff -= 2 * math.pi
self.prev_image = current_image
return PilotCommand(max(min(0.6, angle_diff), -0.6), min(50., distance_to_target / 2))
d_r = max(10, min(75., distance_to_target / 2))
d_a_limit = d_r / 10 * 0.07
return PilotCommand(max(min(d_a_limit, angle_diff), -d_a_limit), d_r)
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
"""Сбрасывает позицию и угол БПЛА"""