add focus of expansion (foe) detection

src/orisys/util.py

@@ -75,7 +75,7 @@ def draw_arrows(image, optical_flow,fps=True, threshold=5, grid_spacing=40, arro
             cv2.arrowedLine(image, (x_axis[i], y_axis[i]), (end_x[i], end_y[i]), color, thickness=thickness, tipLength=0.3)
     return image
 
-def draw_magnitude_map(image, flow_field, threshold=3.0, colormap=cv2.COLORMAP_JET):
+def draw_magnitude_map(image, flow_field, threshold=3.0, colormap=cv2.COLORMAP_JET,downsample_factor=10):
     """
     Draw color map of pointwise magnitude of velocity field.
     
@@ -92,7 +92,6 @@ def draw_magnitude_map(image, flow_field, threshold=3.0, colormap=cv2.COLORMAP_J
     magnitude = np.sqrt(flow_field[:, :, 0]**2 + flow_field[:, :, 1]**2)
     
     # Smooth by downsampling and interpolating back
-    downsample_factor = 10
     h, w = magnitude.shape
     magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor]
     magnitude_smoothed = cv2.resize(magnitude_downsampled, (w, h), interpolation=cv2.INTER_CUBIC)
@@ -130,4 +129,92 @@ def draw_fps(image, fps):
     # Draw FPS text
     cv2.putText(image, fps_text, position, font, font_scale, color, thickness, cv2.LINE_AA)
     
+    return image
+
+
+def draw_foe(image, foe_result, point_color=(0, 255, 0), region_color=(0, 255, 255)):
+    """
+    在图像上绘制扩张焦点和扩张区域。
+    
+    Args:
+        image: 背景图像
+        foe_result: compute_foe 函数的返回结果
+        point_color: 焦点点的颜色 (BGR)
+        region_color: 扩张区域轮廓的颜色 (BGR)
+        
+    Returns:
+        np.ndarray: 绘制了焦点和区域的图像
+    """
+    image = image.copy()
+    
+    # 绘制扩张区域轮廓
+    if 'expansion_region' in foe_result:
+        cv2.drawContours(image, [foe_result['expansion_region']], -1, region_color, 2)
+    
+    # 绘制焦点点
+    if 'foe_point' in foe_result:
+        cx, cy = foe_result['foe_point']
+        cv2.circle(image, (cx, cy), 8, point_color, -1)
+        cv2.circle(image, (cx, cy), 10, point_color, 2)
+        cv2.putText(image, "FOE", (cx + 15, cy), 
+                   cv2.FONT_HERSHEY_SIMPLEX, 0.6, point_color, 2)
+    
+    return image
+
+
+
+def draw_multiple_foe(image, foe_result, point_color=(0, 255, 0), 
+                       region_color=(0, 255, 255), label_colors=None):
+    """
+    在图像上绘制多个扩张焦点和扩张区域。
+    
+    Args:
+        image: 背景图像
+        foe_result: compute_multiple_foe 函数的返回结果
+        point_color: 默认焦点点的颜色 (BGR)
+        region_color: 扩张区域轮廓的颜色 (BGR)
+        label_colors: 可选的颜色列表，用于不同焦点的标签颜色
+        
+    Returns:
+        np.ndarray: 绘制了多个焦点和区域的图像
+    """
+    image = image.copy()
+    
+    # 默认标签颜色（彩虹色系）
+    if label_colors is None:
+        label_colors = [
+            (255, 0, 0),    # 蓝色
+            (0, 255, 0),    # 绿色
+            (0, 0, 255),    # 红色
+            (255, 255, 0),  # 青色
+            (255, 0, 255),  # 洋红色
+            (0, 255, 255),  # 黄色
+            (255, 128, 0),  # 橙色
+            (128, 0, 255)   # 紫色
+        ]
+    
+    # 绘制扩张区域轮廓
+    if 'expansion_regions' in foe_result and foe_result['expansion_regions']:
+        cv2.drawContours(image, foe_result['expansion_regions'], -1, region_color, 2)
+    
+    # 绘制焦点点并添加标签
+    if 'foe_points' in foe_result:
+        for i, (cx, cy) in enumerate(foe_result['foe_points']):
+            # 选择颜色（循环使用颜色列表）
+            color = label_colors[i % len(label_colors)]
+            
+            # 绘制焦点点
+            cv2.circle(image, (cx, cy), 8, color, -1)
+            cv2.circle(image, (cx, cy), 10, color, 2)
+            
+            # 添加标签
+            label = f"FOE{i+1}"
+            cv2.putText(image, label, (cx + 15, cy), 
+                       cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
+    
+    # 显示检测到的焦点数量
+    foe_count = len(foe_result.get('foe_points', []))
+    cv2.putText(image, f"FOE Count: {foe_count}", (10, 60), 
+               cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
+    
     return image