feat: improve fps

autopilot.py

@@ -90,7 +90,7 @@ class AutoPilot(Pilot):
         cx, cy = w // 2, h // 2
         
         # Создаем сетку точек для отслеживания (аналогично вашему step=20)
-        step = 20
+        step = 35
         grid_points = []
         for y in range(step, h - step, step):
             for x in range(step, w - step, step):
@@ -103,25 +103,26 @@ class AutoPilot(Pilot):
         
         # Параметры для Lucas-Kanade
         lk_params = dict(
-            winSize=(21, 21),
-            maxLevel=3,
+            winSize=(11, 11),
+            maxLevel=2,
             criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
         )
-        
+
         # Вычисляем разреженный оптический поток
         p1, status, err = cv2.calcOpticalFlowPyrLK(prev_gray, current_gray, p0, None, **lk_params)
         
         # Фильтруем только успешно отслеженные точки
-        good_old = p0[status == 1]
-        good_new = p1[status == 1]
-        
+        good_mask = err < 12.0  # порог ошибки
+        good_old = p0[status == 1 & good_mask]
+        good_new = p1[status == 1 & good_mask]
+
         if len(good_old) < 4:
             return None, None
-        
+
         # Преобразуем в отцентрированные координаты (Y вверх)
         src_pts = np.float32([[x - cx, cy - y] for x, y in good_old]).reshape(-1, 1, 2)
         dst_pts = np.float32([[x - cx, cy - y] for x, y in good_new]).reshape(-1, 1, 2)
-        
+
         return src_pts, dst_pts
 
     def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray):
@@ -130,7 +131,8 @@ class AutoPilot(Pilot):
         Точки уже отцентрированы относительно центра изображения
         """
         # Используем RANSAC для оценки матрицы гомографии
-        H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0)
+        print("Count of points: ", len(src_pts), len(dst_pts))
+        H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
         # RH, rmask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 1.0)
         
         if H is None:
@@ -153,7 +155,7 @@ class AutoPilot(Pilot):
                 rmse = float(np.sqrt(np.mean(errs ** 2)))
         except Exception:
             rmse = None
-        
+
         # Извлекаем параметры трансформации из матрицы гомографии
         # H = [a11 a12 tx]
         #     [a21 a22 ty]
@@ -266,12 +268,21 @@ class AutoPilot(Pilot):
         )
         
         # Вычисляем оптический поток для покадрового сравнения
+        optical_flow_timer = Timer()
+        optical_flow_timer.start()
         src_pts, dst_pts = self.calculate_optical_flow(self.prev_chunk, current_chunk)
+        optical_flow_timer.stop()
+        print(f"Optical flow calculating: {optical_flow_timer.get_elapsed() * 1000:.2f} ms")
 
         # Оцениваем смещение БПЛА
         if src_pts is not None and dst_pts is not None:
             # Оцениваем матрицу трансформации
+            
+            matrix_estimation_timer = Timer()
+            matrix_estimation_timer.start()
             transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
+            matrix_estimation_timer.stop()
+            print(f"Transformation matrix estimating: {matrix_estimation_timer.get_elapsed() * 1000:.2f} ms")
             
             if transformation_info:
                 # Обновляем позицию и угол БПЛА (одометрия по кадрам)
@@ -290,11 +301,17 @@ class AutoPilot(Pilot):
 
                 self.timer.start()
 
+        
+        chunk_timer = Timer()
+        chunk_timer.start()
         # Пытаемся найти ориентир на картинке:
         self.prev_chunk = current_chunk
-        npos = self.get_position_by_chunk()
-        if npos is not None:
-            self.reserved_geo = npos
+        # npos = self.get_position_by_chunk()
+        # if npos is not None:
+        #     self.reserved_geo = npos
+
+        chunk_timer.stop()
+        print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
 
         if distance_to_target < 35:
             self.target_idx += 1