feat: add chunks to autopilot, test for estimation

visualization.py

@@ -31,6 +31,7 @@ class VisualizationManager:
         self.ax_global_map = None
         self.ax_detection = None
         self.ax_matches = None
+        self.ax_chunk_matches = None
         
         # Данные для глобальной карты
         self.trajectory_x = []
@@ -64,7 +65,7 @@ class VisualizationManager:
         self.fig.canvas.manager.window.state('zoomed')
         
         # Создаем сетку 2x2 с разными размерами колонок
-        gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1])
+        gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[.5, 1, 1])
         
         # График погрешности позиции (левый верхний угол)
         self.ax_error_plot = self.fig.add_subplot(gs[0, 0])
@@ -92,10 +93,16 @@ class VisualizationManager:
         self.ax_matches.set_title('Feature Matching')
         self.ax_matches.axis('off')
         
+        # Сопоставление точек (правый нижний угол)
+        self.ax_chunk_matches = self.fig.add_subplot(gs[0, 2])
+        self.ax_chunk_matches.set_title('Chunk Matching')
+        self.ax_chunk_matches.axis('off')
+        
         # Настройки окна
         self.fig.canvas.manager.window.attributes('-topmost', False)
         
         plt.tight_layout()
+        plt.show(block=False)
     
     def update_global_map(self, x: float, y: float, mode: SimMode):
         """Обновляет глобальную карту"""
@@ -257,6 +264,40 @@ class VisualizationManager:
         
         self.ax_matches.axis('off')
     
+    def update_chunk_matches(self, img1: np.ndarray, img2: np.ndarray, 
+                      kp1, kp2, matches, transformation_info=None):
+        """Обновляет визуализацию сопоставления точек"""
+        self.ax_chunk_matches.clear()
+        self.ax_chunk_matches.set_title('Chunk Matching')
+        
+        if img1 is not None and img2 is not None and matches:
+            # Рисуем сопоставления
+            img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None,
+                                         flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
+            
+            # Конвертируем BGR в RGB
+            if len(img_matches.shape) == 3 and img_matches.shape[2] == 3:
+                img_matches_rgb = cv2.cvtColor(img_matches, cv2.COLOR_BGR2RGB)
+            else:
+                img_matches_rgb = img_matches
+            
+            self.ax_chunk_matches.imshow(img_matches_rgb)
+            
+            # Добавляем информацию о трансформации
+            if transformation_info:
+                tx, ty = transformation_info['translation']
+                angle = transformation_info['rotation']
+                
+                info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
+                info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
+                
+                self.ax_chunk_matches.text(10, 30, info_text, fontsize=8, color='green', 
+                                   bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
+                self.ax_chunk_matches.text(10, 90, info_text2, fontsize=8, color='green',
+                                   bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
+        
+        self.ax_chunk_matches.axis('off')
+    
     def update_display(self):
         """Обновляет отображение всех областей"""
         self.fig.canvas.draw()