feat: dynamic map; refactor code and fix bugs

position.py

@@ -42,10 +42,32 @@ class Position:
             f"roll={math.degrees(self.roll):.1f}°)"
         )
 
+    def __imul__(self, scalar: float):
+        self.x *= scalar
+        self.y *= scalar
+        self.z *= scalar
+        return self
+
+    def __mul__(self, scalar: float) -> 'Position':
+        pos = self.copy()
+        pos *= scalar
+        return pos
+
+    def __itruediv__(self, scalar: float):
+        self.x /= scalar
+        self.y /= scalar
+        self.z /= scalar
+        return self
+
+    def __truediv__(self, scalar: float) -> 'Position':
+        pos = self.copy()
+        pos /= scalar
+        return pos
+
     def get_homography_matrix(self, K_in: np.ndarray = constants.K, K_out: np.ndarray | None = None, sliding: bool = True) -> np.ndarray:
         """ Возвращает матрицу гомографии """
         R = self.get_rotation_matrix()
-        T = self.get_translation_matrix()
+        T = self.get_translation_matrix(K_in)
         if not sliding:
             T[0, 2] = T[1, 2] = 0
         if K_out is None: K_out = K_in
@@ -55,10 +77,10 @@ class Position:
         """Создает полную копию объекта"""
         return Position(self.x, self.y, self.z, self.yaw, self.pitch, self.roll)
     
-    def get_translation_matrix(self) -> np.ndarray:
+    def get_translation_matrix(self, K: np.ndarray = constants.K) -> np.ndarray:
         return np.array([
-            [1, 0, self.x / constants._K_FOCUS_DISTANCE],
-            [0, 1, self.y / constants._K_FOCUS_DISTANCE],
+            [1, 0, self.x / K[0][0]],
+            [0, 1, self.y / K[0][0]],
             [0, 0, self.z]
         ])
 
@@ -102,6 +124,13 @@ class Position:
         R = np.array(R)
         t = np.array(t)
 
+        # print(cv2.decomposeHomographyMat(inv(H), K))
+        # _, _, z, _ = cv2.decomposeHomographyMat(inv(H), K)
+        # print(z)
+        # z = z.copy()
+        # z *= constants._K_FOCUS_DISTANCE
+        # print(z)
+        
         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)
@@ -122,9 +151,11 @@ class Position:
         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.x -= T[0] * constants._K_FOCUS_DISTANCE
+        self.y += T[1] * constants._K_FOCUS_DISTANCE
         self.z = 1 + T[2]
+        T[0] *= constants._K_FOCUS_DISTANCE
+        T[1] *= constants._K_FOCUS_DISTANCE
 
     def apply(self, homography_matrix: np.ndarray, K = constants.K) -> 'Position':
         """Применяет матрицу трансформации для вычисления новой позиции и ориентации."""