add focus of expansion (foe) detection

src/orisys/get_depth.py

@@ -0,0 +1,154 @@
+import numpy as np
+import cv2
+
+def compute_foe(flow_field, min_magnitude=1.0, expansion_threshold=1, downsample_factor=10, return_all_contours=False,kernel_size=(5,5)):
+    """
+    计算光流场的扩张焦点(Focus of Expansion)。
+    
+    Args:
+        flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
+        min_magnitude: 最小光流幅度阈值，低于此值的点不参与计算
+        expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
+        downsample_factor: 下采样因子，用于平滑计算（默认10）
+        return_all_contours: 是否返回所有轮廓（默认False，仅返回最大轮廓）
+        
+    Returns:
+        dict: 包含扩张焦点信息的字典，包括：
+            - 'foe_point': 扩张焦点坐标 (x, y)，如果找到单个焦点
+            - 'expansion_region': 扩张区域轮廓（最大轮廓）
+            - 'expansion_mask': 扩张区域二值掩码
+            - 'contraction_mask': 收缩区域二值掩码
+            - 'divergence_map': 散度图
+            - '_all_contours': 所有轮廓列表（仅当return_all_contours=True时）
+    """
+    H, W = flow_field.shape[:2]
+    
+    # 平滑处理：下采样光流场，然后计算并插值回原尺寸
+    if downsample_factor > 1:
+        # 下采样光流场
+        flow_field_downsampled = flow_field[::downsample_factor, ::downsample_factor, :]
+        h_ds, w_ds = flow_field_downsampled.shape[:2]
+        
+        # 在下采样的光流场上计算幅度和散度
+        u_ds = flow_field_downsampled[:, :, 0]
+        v_ds = flow_field_downsampled[:, :, 1]
+        
+        # 计算下采样后的光流幅度
+        magnitude_ds = np.sqrt(u_ds**2 + v_ds**2)
+        
+        # 计算下采样后的散度
+        du_dx_ds = np.gradient(u_ds, axis=1)
+        dv_dy_ds = np.gradient(v_ds, axis=0)
+        divergence_ds = du_dx_ds + dv_dy_ds
+        
+        # 插值回原尺寸
+        magnitude = cv2.resize(magnitude_ds, (W, H), interpolation=cv2.INTER_CUBIC)
+        divergence = cv2.resize(divergence_ds, (W, H), interpolation=cv2.INTER_CUBIC)
+    else:
+        # 不进行下采样，直接计算
+        # 计算光流幅度
+        magnitude = np.sqrt(flow_field[:, :, 0]**2 + flow_field[:, :, 1]**2)
+        
+        # 计算散度 (divergence)
+        u = flow_field[:, :, 0]
+        v = flow_field[:, :, 1]
+        
+        # 使用中心差分计算散度
+        du_dx = np.gradient(u, axis=1)
+        dv_dy = np.gradient(v, axis=0)
+        divergence = du_dx + dv_dy
+    
+    # 创建有效点掩码（幅度足够大的点）
+    valid_mask = magnitude > min_magnitude
+    
+    # 创建扩张和收缩区域掩码
+    expansion_mask = np.zeros_like(divergence, dtype=bool)
+    contraction_mask = np.zeros_like(divergence, dtype=bool)
+    
+    expansion_mask[valid_mask] = divergence[valid_mask] > expansion_threshold
+    contraction_mask[valid_mask] = divergence[valid_mask] < -expansion_threshold
+    
+    result = {
+        'expansion_mask': expansion_mask,
+        'contraction_mask': contraction_mask,
+        'divergence_map': divergence
+    }
+    
+    # 寻找扩张区域轮廓
+    expansion_binary = expansion_mask.astype(np.uint8) * 255
+    if np.any(expansion_binary):
+        # 应用形态学闭运算来填充小孔和连接邻近区域
+        kernel = np.ones(kernel_size, np.uint8)
+        expansion_binary_closed = cv2.morphologyEx(expansion_binary, cv2.MORPH_CLOSE, kernel)
+        
+        contours, _ = cv2.findContours(expansion_binary_closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        if contours:
+            # 如果请求返回所有轮廓，保存它们
+            if return_all_contours:
+                result['_all_contours'] = contours
+            
+            # 找到最大的扩张区域轮廓
+            largest_contour = max(contours, key=cv2.contourArea)
+            result['expansion_region'] = largest_contour
+            
+            # 计算扩张区域的质心作为焦点
+            M = cv2.moments(largest_contour)
+            if M["m00"] > 0:
+                cx = int(M["m10"] / M["m00"])
+                cy = int(M["m01"] / M["m00"])
+                result['foe_point'] = (cx, cy)
+    
+    return result
+
+def compute_multiple_foe(flow_field, min_magnitude=1.0, expansion_threshold=0.001, 
+                         downsample_factor=10, min_contour_area=3000):
+    """
+    计算光流场的多个扩张焦点(Foci of Expansion)。
+    
+    Args:
+        flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
+        min_magnitude: 最小光流幅度阈值，低于此值的点不参与计算
+        expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
+        downsample_factor: 下采样因子，用于平滑计算（默认10）
+        min_contour_area: 最小轮廓面积阈值，小于此值的轮廓会被忽略
+        
+    Returns:
+        dict: 包含多个扩张焦点信息的字典，包括：
+            - 'foe_points': 扩张焦点坐标列表 [(x1, y1), (x2, y2), ...]
+            - 'expansion_regions': 扩张区域轮廓列表
+            - 'expansion_mask': 扩张区域二值掩码
+            - 'contraction_mask': 收缩区域二值掩码
+            - 'divergence_map': 散度图
+    """
+    # 调用 compute_foe 获取基础数据和所有轮廓
+    foe_result = compute_foe(
+        flow_field, 
+        min_magnitude=min_magnitude,
+        expansion_threshold=expansion_threshold,
+        downsample_factor=downsample_factor,
+        return_all_contours=True
+    )
+    
+    # 构建多焦点结果，复用基础数据
+    result = {
+        'expansion_mask': foe_result['expansion_mask'],
+        'contraction_mask': foe_result['contraction_mask'],
+        'divergence_map': foe_result['divergence_map'],
+        'foe_points': [],
+        'expansion_regions': []
+    }
+    
+    # 处理所有轮廓，过滤并计算焦点
+    if '_all_contours' in foe_result:
+        for contour in foe_result['_all_contours']:
+            # 过滤掉太小的轮廓
+            if cv2.contourArea(contour) >= min_contour_area:
+                # 计算轮廓的质心作为焦点
+                M = cv2.moments(contour)
+                if M["m00"] > 0:
+                    cx = int(M["m10"] / M["m00"])
+                    cy = int(M["m01"] / M["m00"])
+                    result['foe_points'].append((cx, cy))
+                    result['expansion_regions'].append(contour)
+    
+    return result

src/orisys/sensor.py

@@ -20,6 +20,7 @@ from .tracker import Tracker
 from .nHHD import nHHD
 from .getDensity import getFilteredDensity
 from . import configs 
+from .get_depth import *
 
 
 class Sensor:
@@ -93,7 +94,7 @@ class Sensor:
         self.img = None
         self.img_filtered = None
         self.flow = None
-        self.normal = None
+        self.vnormal = None
         self.shear = None
         self.depth = None
         self.n_boxes = None
@@ -217,13 +218,13 @@ class Sensor:
         if log_timing: timings['hhd_decompose'] = (time.perf_counter() - t0) * 1000
         
         # Get components
-        self.normal = self.hhd.get_normal()
+        self.vnormal = self.hhd.get_normal()
         self.shear = self.hhd.get_shear()
         
-        # Depth computation
-        if log_timing: t0 = time.perf_counter()
-        self.depth = getFilteredDensity(flow, use_cuda=self.cuda)
-        if log_timing: timings['get_density'] = (time.perf_counter() - t0) * 1000
+        # # Depth computation
+        # if log_timing: t0 = time.perf_counter()
+        # self.depth = getFilteredDensity(self.vnormal, use_cuda=self.cuda)
+        # if log_timing: timings['get_density'] = (time.perf_counter() - t0) * 1000
         
         # Update state
         self.flow = flow
@@ -240,6 +241,13 @@ class Sensor:
 
         return self.flow
 
+    def get_foe(self, multiple_foe=False, min_contour_area=3000, min_magnitude=1.0, expansion_threshold=1, downsample_factor=10,kernel_size=(5,5)):
+        
+        if multiple_foe:
+            return compute_multiple_foe(self.vnormal, min_contour_area=min_contour_area, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor,kernel_size=kernel_size)
+        else:
+            return compute_foe(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor,kernel_size=kernel_size)
+
     def update(self):
         """
         Convenience method: get_img + compute_deformation in one call.
@@ -251,7 +259,7 @@ class Sensor:
         self.compute_deformation()
         return {
             'img': self.img, 'flow': self.flow,
-            'normal': self.normal, 'shear': self.shear, 'depth': self.depth,
+            'normal': self.vnormal, 'shear': self.shear, 'depth': self.depth,
             'fps': self.fps, 'fnormal': self.fnormal,
             'fshearx': self.fshearx, 'fsheary': self.fsheary, 'ftor': self.ftor,
         }
@@ -271,7 +279,7 @@ class Sensor:
         
         mapping = {
             self.info.VRAW: lambda: self.flow if self.flow is not None else np.array([]),
-            self.info.VNORMAL: lambda: self.normal if self.normal is not None else np.array([]),
+            self.info.VNORMAL: lambda: self.vnormal if self.vnormal is not None else np.array([]),
             self.info.VSHEAR: lambda: self.shear if self.shear is not None else np.array([]),
             self.info.FNORMAL: lambda: self.fnormal,
             self.info.FSHEARX: lambda: self.fshearx,
@@ -343,7 +351,8 @@ class Sensor:
             self.last_log_time = t
             
             # Sync instance variables
-            for k in ['img', 'flow', 'normal', 'shear', 'depth', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
+            # for k in ['img', 'flow', 'normal', 'shear', 'depth', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
+            for k in ['img', 'flow', 'normal', 'shear', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
                 setattr(self, k, result.get(k))
             self.vupdate = True
             return result
@@ -543,7 +552,7 @@ class Sensor:
                 self._queue_put(self._result_queue, {
                     'img': img, 'flow': flow,
                     'normal': self.hhd.get_normal(), 'shear': self.hhd.get_shear(),
-                    'depth': getFilteredDensity(flow, use_cuda=self.cuda),
+                    # 'depth': getFilteredDensity(flow, use_cuda=self.cuda),
                     'fnormal': forces[0], 'fshearx': forces[1],
                     'fsheary': forces[2], 'ftor': forces[3],
                 })

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)
@@ -131,3 +130,91 @@ def draw_fps(image, fps):
     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