diff --git a/geolocation.py b/geolocation.py deleted file mode 100644 index 7d6b377..0000000 --- a/geolocation.py +++ /dev/null @@ -1,213 +0,0 @@ - -import math -import numpy as np -from typing import Optional - - -class Geolocation: - """Класс геопозиции с полной ориентацией БПЛА в 3D пространстве""" - - x: float # Координата X (метры) - y: float # Координата Y (метры) - z: float # Масштаб (высота) - yaw: float # Рыскание (rotation around Z-axis) - pitch: float # Тангаж (rotation around Y-axis) - roll: float # Крен (rotation around X-axis) - - def __init__( - self, - x: float = 0, - y: float = 0, - z: float = 1, - yaw: float = 0, - pitch: float = 0, - roll: float = 0 - ): - self.x = x - self.y = y - self.z = z - self.yaw = yaw - self.pitch = pitch - self.roll = roll - - def __str__(self) -> str: - return ( - f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}, " - f"yaw={math.degrees(self.yaw):.1f}°, " - f"pitch={math.degrees(self.pitch):.1f}°, " - f"roll={math.degrees(self.roll):.1f}°)" - ) - - def __matmul__(self, mat: np.ndarray) -> 'Geolocation': - return self.copy().apply(mat) - - def __imatmul__(self, mat: np.ndarray) -> 'Geolocation': - return self.apply(mat) - - def apply(self, mat: np.ndarray) -> 'Geolocation': - """ - Применяет матрицу трансформации для вычисления новой позиции и ориентации. - - Для 2D трансформаций (3x3 матрица) обновляет только x, y, z и yaw. - Для 3D трансформаций (4x4 матрица) обновляет все параметры включая pitch и roll. - """ - if mat.shape == (3, 3): - return self._apply_2d(mat) - elif mat.shape == (4, 4): - return self._apply_3d(mat) - else: - raise ValueError(f"Unsupported matrix shape: {mat.shape}. Expected (3,3) or (4,4)") - - def _apply_2d(self, mat: np.ndarray) -> 'Geolocation': - """Применяет 2D трансформацию""" - tx, ty = -mat[0, 2], -mat[1, 2] - rotation = -np.arctan2(mat[1, 0], mat[0, 0]) - scale = np.hypot(mat[0, 0], mat[0, 1]) - - dx_meters = tx - dy_meters = ty - angle_global = self.yaw + rotation - - cos_angle = math.cos(angle_global) - sin_angle = math.sin(angle_global) - - dx_global = dx_meters * cos_angle - dy_meters * sin_angle - dy_global = dx_meters * sin_angle + dy_meters * cos_angle - - self.z /= scale if scale != 0 else 1 - self.x += dx_global * self.z - self.y += dy_global * self.z - self.yaw = self._normalize_angle(angle_global) - - return self - - def _apply_3d(self, mat: np.ndarray) -> 'Geolocation': - """Применяет 3D трансформацию с извлечением углов Эйлера""" - position = np.array([self.x, self.y, 0, 1]) - new_position = mat @ position - - self.x = new_position[0] - self.y = new_position[1] - - rotation_mat = mat[:3, :3] - scale = np.linalg.norm(rotation_mat[:, 0]) - self.z *= scale - - normalized_mat = rotation_mat / scale - yaw, pitch, roll = self._extract_euler_angles(normalized_mat) - - self.yaw = self._normalize_angle(self.yaw + yaw) - self.pitch = self._normalize_angle(self.pitch + pitch) - self.roll = self._normalize_angle(self.roll + roll) - - return self - - @staticmethod - def _extract_euler_angles(R: np.ndarray) -> tuple[float, float, float]: - """ - Извлекает углы Эйлера (yaw, pitch, roll) из матрицы вращения. - Использует ZYX convention (yaw-pitch-roll). - """ - sy = math.sqrt(R[0, 0]**2 + R[1, 0]**2) - - singular = sy < 1e-6 - - if not singular: - roll = math.atan2(R[2, 1], R[2, 2]) - pitch = math.atan2(-R[2, 0], sy) - yaw = math.atan2(R[1, 0], R[0, 0]) - else: - roll = math.atan2(-R[1, 2], R[1, 1]) - pitch = math.atan2(-R[2, 0], sy) - yaw = 0 - - return yaw, pitch, roll - - @staticmethod - def _normalize_angle(angle: float) -> float: - """Нормализует угол в диапазон [-π, π]""" - return math.atan2(math.sin(angle), math.cos(angle)) - - def copy(self) -> 'Geolocation': - """Создает полную копию объекта""" - return Geolocation(self.x, self.y, self.z, self.yaw, self.pitch, self.roll) - - def get_rotation_matrix(self) -> np.ndarray: - """ - Матрица вращения с порядком применения: yaw → pitch → roll - Умножение: R = Rx(roll) * Ry(pitch) * Rz(yaw) - """ - cy, sy = math.cos(self.yaw), math.sin(self.yaw) - cp, sp = math.cos(self.pitch), math.sin(self.pitch) - cr, sr = math.cos(self.roll), math.sin(self.roll) - - # Порядок умножения: Rx * Ry * Rz (справа налево: yaw→pitch→roll) - return np.array([ - [cy*cp, -sy*cr + cy*sp*sr, sy*sr + cy*sp*cr], - [sy*cp, cy*cr + sy*sp*sr, -cy*sr + sy*sp*cr], - [-sp, cp*sr, cp*cr] - ]) - - def to_dict(self) -> dict: - """Экспорт в словарь""" - return { - 'x': self.x, - 'y': self.y, - 'z': self.z, - 'yaw': self.yaw, - 'pitch': self.pitch, - 'roll': self.roll - } - - @classmethod - def from_dict(cls, data: dict) -> 'Geolocation': - """Импорт из словаря""" - return cls( - x=data.get('x', 0), - y=data.get('y', 0), - z=data.get('z', 1), - yaw=data.get('yaw', 0), - pitch=data.get('pitch', 0), - roll=data.get('roll', 0) - ) - - @staticmethod - def transform(g: 'Geolocation', mat: np.ndarray) -> Optional['Geolocation']: - """Статический метод трансформации (создает копию и применяет матрицу)""" - if g is None: - return None - return g.copy().apply(mat) - - -# Пример использования -if __name__ == "__main__": - # Создание геопозиции - geo = Geolocation(x=100, y=200, z=50, yaw=math.radians(45), pitch=math.radians(10), roll=math.radians(5)) - print("Исходная позиция:", geo) - - # 2D трансформация (обратная совместимость) - mat_2d = np.array([ - [0.9, -0.1, 5], - [0.1, 0.9, 10], - [0, 0, 1] - ]) - geo_2d = geo @ mat_2d - print("\nПосле 2D трансформации:", geo_2d) - - # 3D трансформация - angle = math.radians(30) - mat_3d = np.array([ - [math.cos(angle), -math.sin(angle), 0, 15], - [math.sin(angle), math.cos(angle), 0, 20], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - geo_3d = geo.copy() - geo_3d @= mat_3d - print("\nПосле 3D трансформации:", geo_3d) - - # Получение матрицы вращения - rot_mat = geo.get_rotation_matrix() - print("\nМатрица вращения:\n", rot_mat) - - print("\n✓ Код успешно выполнен") diff --git a/position.py b/position.py new file mode 100644 index 0000000..3cc897b --- /dev/null +++ b/position.py @@ -0,0 +1,132 @@ + +import cv2 +import math +import numpy as np +from numpy.linalg import inv +from typing import Optional +import constants +from scipy.spatial.transform import Rotation +import utility + +class Position: + """Класс позиции с полной ориентацией БПЛА в 3D пространстве""" + + x: float # Координата X + y: float # Координата Y + z: float # Масштаб + yaw: float # Рыскание (rotation around Z-axis) + pitch: float # Тангаж (rotation around Y-axis) + roll: float # Крен (rotation around X-axis) + + def __init__( + self, + x: float = 0, + y: float = 0, + z: float = 1, + yaw: float = 0, + pitch: float = 0, + roll: float = 0 + ): + self.x = x + self.y = y + self.z = z + self.yaw = yaw + self.pitch = pitch + self.roll = roll + + def __str__(self) -> str: + return ( + f"Position(x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}, " + f"yaw={math.degrees(self.yaw):.1f}°, " + f"pitch={math.degrees(self.pitch):.1f}°, " + f"roll={math.degrees(self.roll):.1f}°)" + ) + + def get_homography_matrix(self, K: np.ndarray = constants.K, sliding: bool = True) -> np.ndarray: + """ Возвращает матрицу гомографии """ + R = self.get_rotation_matrix() + T = self.get_translation_matrix() + if not sliding: + T[0, 2] = T[1, 2] = 0 + return K @ R @ T @ np.linalg.inv(K) + + def copy(self) -> 'Position': + """Создает полную копию объекта""" + return Position(self.x, self.y, self.z, self.yaw, self.pitch, self.roll) + + def get_translation_matrix(self) -> np.ndarray: + return np.array([ + [1, 0, self.x / constants._K_FOCUS_DISTANCE], + [0, 1, self.y / constants._K_FOCUS_DISTANCE], + [0, 0, self.z] + ]) + + def get_rotation_matrix(self) -> np.ndarray: + """ + Матрица вращения с порядком применения: yaw → pitch → roll + Умножение: R = Rx(roll) * Ry(pitch) * Rz(yaw) + """ + cy, sy = math.cos(self.yaw), math.sin(self.yaw) + cp, sp = math.cos(self.pitch), math.sin(self.pitch) + cr, sr = math.cos(self.roll), math.sin(self.roll) + + Rz = np.array([ + [cy, -sy, 0], + [sy, cy, 0], + [0, 0, 1], + ]) + + Ry = np.array([ + [cp, 0, sp], + [0, 1, 0], + [-sp, 0, cp], + ]) + + Rx = np.array([ + [1, 0, 0], + [0, cr, -sr], + [0, sr, cr], + ]) + + return Rx @ Ry @ Rz + + def iapply(self, homography_matrix: np.ndarray, K = constants.K) -> 'Position': + """Применяет матрицу трансформации для вычисления новой позиции и ориентации.""" + + np.set_printoptions(suppress=True) + H = homography_matrix @ self.get_homography_matrix(sliding=False) + + # Decompose homography + _, R, t, _ = cv2.decomposeHomographyMat(H, K) + R = np.array(R) + t = np.array(t) + + T = inv(R) @ inv(K) @ H @ K + ind = np.array([A[2][0] ** 2 + A[2][1] ** 2 for A in T]) + top_k = max(1, len(T) // 2) + if (len(T) == 3): raise "len(T) == 3" + ind = np.argpartition(ind, top_k - 1)[:top_k] + T = T[ind[0]] + T = T @ np.array([0, 0, 1]) / np.mean((T[0][0], T[1][1])) + T[2] -= 1 + R = R[ind] + t = t[ind] + + best_id = ((t - T) ** 2).sum((1, 2)).argmin() + + R = R[best_id] + rot = Rotation.from_matrix(R).as_euler('XYZ').flatten() + self.roll = rot[0] + self.pitch = rot[1] + self.yaw = rot[2] + + t = t[best_id].flatten() + self.x += -T[0] * constants._K_FOCUS_DISTANCE * self.z + self.y += T[1] * constants._K_FOCUS_DISTANCE * self.z + self.z = 1 + T[2] + + def apply(self, homography_matrix: np.ndarray, K = constants.K) -> 'Position': + """Применяет матрицу трансформации для вычисления новой позиции и ориентации.""" + pos = self.copy() + pos.iapply(homography_matrix, K) + return pos