add config dictionary to get_depth.py

example_contact.py

@@ -10,7 +10,7 @@ def main():
     # sensor = orisys.Sensor("./s1.mp4", isstitch=True, cuda=True, verbose=True)
     
     # 方式2: 从摄像头读取（取消注释使用）
-    sensor = orisys.Sensor(0, isstitch=True, cuda=True, verbose=True)
+    sensor = orisys.Sensor(0, isstitch=True, cuda=False, verbose=True)
 
     print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")
 
@@ -37,7 +37,8 @@ def main():
         cv2.imshow("形变场 Deformation", arrows)
 
         # 计算触碰点
-        sensor.compute_contact(min_magnitude=0.1,expansion_threshold=0.01,denoise_sigma=15)
+        # sensor.compute_contact(min_magnitude=0.1,expansion_threshold=0.01,denoise_sigma=15)
+        sensor.compute_contact()
         
         # 在图像上触碰点和区域
         image_with_foe = orisys.util.draw_contact(sensor.img.copy(), sensor.contour, sensor.centroid)

src/orisys/Stitcher.py

@@ -6,15 +6,15 @@ import cv2
 
 class Stitcher:
 
-    def __init__(self,isstitch=False, n=[3,3], index=np.array([1,2,3,6,7,8,11,12,13])):
+    def __init__(self,isstitch=False, n=[3,3], index=np.array([1,2,3,6,7,8,11,12,13]), config_stitcher=None):
 
         # Settings
         self.isstitch = isstitch
-
+        self.config_stitcher = config_stitcher
         # Find Boxes Related
-        self.min_area_frac = 0.01
+        self.min_area_frac = config_stitcher["min_area_frac"]
-        self.max_area_frac = 0.05
+        self.max_area_frac = config_stitcher["max_area_frac"]
-        self.max_aspect_ratio = 100
+        self.max_aspect_ratio = config_stitcher["max_aspect_ratio"]
 
         # stitch related
         self.stitch_valid = False

src/orisys/configs/prototype.json

@@ -17,7 +17,8 @@
     },
     "contact": {
       "min_contour_area": 3000,
-      "min_magnitude": 0.02,
+      "flow_threshold":1,
+      "div_threshold":0.02,
       "expansion_threshold": 0.01,
       "downsample_factor": 1,
       "kernel_size": [5,5],

src/orisys/get_depth.py

@@ -12,325 +12,346 @@ except ImportError:
 # 缓存形态学操作的核，避免重复创建
 _morphology_kernels = {}
 
-def _create_foe_result(expansion_mask, contraction_mask, divergence_map, 
+class Contact3D:
-                       centroid=None, centroid_found=False, contour=None, 
+    def __init__(self, config_contact=None, verbose=False):
-                       all_contours=None):
-    """
-    创建标准化的FOE结果字典，统一所有函数的返回格式。
-    
-    Args:
-        expansion_mask: 扩张区域掩码
-        contraction_mask: 收缩区域掩码
-        divergence_map: 散度图
-        centroid: 质心坐标，可以是单个元组 (x, y) 或列表 [(x1, y1), (x2, y2), ...]
-        centroid_found: 是否找到质心
-        contour: 轮廓，可以是单个轮廓或轮廓列表
-        all_contours: 所有轮廓（仅在return_all_contours=True时使用）
-    
-    Returns:
-        dict: 标准化的FOE结果字典
-    """
-    result = {
-        'expansion_mask': expansion_mask,
-        'contraction_mask': contraction_mask,
-        'divergence_map': divergence_map,
-        'centroid': centroid if centroid is not None else [0,0],
-        'centroid_found': centroid_found,
-    }
-    
-    # 可选字段
-    if contour is not None:
-        result['contour'] = contour
-    
-    if all_contours is not None:
-        result['_all_contours'] = all_contours
-    
-    return result
 
-def compute_foe(flow_field, min_magnitude=1.0, expansion_threshold=1, downsample_factor=1, return_all_contours=False, kernel_size=(5,5), denoise_sigma=10):
+        # Load Config
-    """
+        self.config_contact = config_contact
-    计算光流场的扩张焦点(Focus of Expansion)。
+        self.flow_threshold = config_contact["flow_threshold"]
-    
+        self.expansion_threshold = config_contact["expansion_threshold"]
-    Args:
+        self.downsample_factor = config_contact["downsample_factor"]
-        flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
+        self.kernel_size = config_contact["kernel_size"]
-        min_magnitude: 最小光流幅度阈值，低于此值的点不参与计算
+        self.denoise_sigma = config_contact["denoise_sigma"]
-        expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
+        self.min_contour_area = config_contact["min_contour_area"]
-        downsample_factor: 下采样因子，用于平滑计算（默认10）
+        # Divergence threshold: only keep divergence values above this threshold, set others to zero
-        return_all_contours: 是否返回所有轮廓（默认False，仅返回最大轮廓）
+        self.divergence_threshold = config_contact["div_threshold"]
-        kernel_size: 形态学操作的核大小
+        self.verbose = verbose
-        denoise_sigma: 降噪高斯滤波的标准差（默认1.5，设为0禁用降噪）
+        if self.verbose:
-        
+            print(f"Contact3D initialized with config: {self.config_contact}")
-    Returns:
-        dict: 包含扩张焦点信息的字典
-    """
-    H, W = flow_field.shape[:2]
-    
-    # 优化：使用视图而不是复制，减少内存分配
-    u = flow_field[:, :, 0]
-    v = flow_field[:, :, 1]
-    
-    # 下采样并计算散度，然后插值回原尺寸
-    if downsample_factor > 1:
-        # 使用步进切片，避免复制
-        u_ds = u[::downsample_factor, ::downsample_factor]
-        v_ds = v[::downsample_factor, ::downsample_factor]
-        
-        # 计算幅度（优化：使用square减少中间数组）
-        magnitude_ds = np.sqrt(np.square(u_ds) + np.square(v_ds))
-        if denoise_sigma > 0:
-            magnitude_ds = cv2.GaussianBlur(magnitude_ds, (0, 0), denoise_sigma)
-        
-        # 优化：合并梯度计算，减少中间数组
-        # 计算散度：du/dx + dv/dy
-        du_dx = np.gradient(u_ds, axis=1)
-        dv_dy = np.gradient(v_ds, axis=0)
-        divergence_ds = du_dx + dv_dy  # 直接相加，避免中间数组
-        
-        if denoise_sigma > 0:
-            divergence_ds = cv2.GaussianBlur(divergence_ds, (0, 0), denoise_sigma)
-        
-        # 插值回原尺寸
-        magnitude = cv2.resize(magnitude_ds, (W, H), interpolation=cv2.INTER_CUBIC)
-        divergence = cv2.resize(divergence_ds, (W, H), interpolation=cv2.INTER_CUBIC)
-    else:
-        # 计算幅度（优化：使用square减少中间数组）
-        magnitude = np.sqrt(np.square(u) + np.square(v))
-        if denoise_sigma > 0:
-            magnitude = cv2.GaussianBlur(magnitude, (0, 0), denoise_sigma)
-        
-        # 优化：合并梯度计算
-        du_dx = np.gradient(u, axis=1)
-        dv_dy = np.gradient(v, axis=0)
-        divergence = du_dx + dv_dy
-        
-        # 降噪：在计算散度之后对散度图进行平滑
-        if denoise_sigma > 0:
-            divergence = cv2.GaussianBlur(divergence, (0, 0), denoise_sigma)
-    
-    # 优化：使用原地操作
-    np.where(divergence > min_magnitude, divergence, 0, out=divergence)
-    
-    # 创建掩码（使用原地操作优化）
-    valid_mask = magnitude > min_magnitude
-    expansion_mask = (valid_mask & (divergence > expansion_threshold))
-    contraction_mask = (valid_mask & (divergence < -expansion_threshold))
-    
-    # 初始化结果字典
-    result = _create_foe_result(expansion_mask, contraction_mask, divergence)
-    
-    # 寻找扩张区域轮廓
-    # 优化：缓存形态学操作的核
-    kernel_key = kernel_size
-    if kernel_key not in _morphology_kernels:
-        _morphology_kernels[kernel_key] = np.ones(kernel_size, np.uint8)
-    
-    expansion_binary = expansion_mask.astype(np.uint8) * 255
-    if np.any(expansion_binary):
-        expansion_binary_closed = cv2.morphologyEx(expansion_binary, cv2.MORPH_CLOSE, _morphology_kernels[kernel_key])
-        contours, _ = cv2.findContours(expansion_binary_closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-        
-        if contours:
-            result['centroid_found'] = True
-            if return_all_contours:
-                result['_all_contours'] = contours
-            
-            largest_contour = max(contours, key=cv2.contourArea)
-            result['contour'] = largest_contour
-            
-            M = cv2.moments(largest_contour)
-            if M["m00"] > 0:
-                result['centroid'] = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
-    
-    return result
 
-def compute_foe_gpu(flow_field, min_magnitude=1.0, expansion_threshold=1, downsample_factor=1, 
+        # Data Variables
-                    return_all_contours=False, kernel_size=(5,5), denoise_sigma=10):
+        self.foe_results = None
-    """
+        # self.expansion_mask = None
-    使用CUDA加速计算光流场的扩张焦点(Focus of Expansion)。
+        # self.contraction_mask = None
-    
+        # self.divergence_map = None
-    Args:
+        # self.centroid = None
-        flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
+        # self.centroid_found = False
-        min_magnitude: 最小光流幅度阈值，低于此值的点不参与计算
+        # self.contour = None
-        expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
+        # self.all_contours = None
-        downsample_factor: 下采样因子，用于平滑计算（默认10）
-        return_all_contours: 是否返回所有轮廓（默认False，仅返回最大轮廓）
-        kernel_size: 形态学操作的核大小
-        denoise_sigma: 降噪高斯滤波的标准差（默认1.5，设为0禁用降噪）
-        
-    Returns:
-        dict: 包含扩张焦点信息的字典
-        
-    Note:
-        如果CuPy不可用，会自动回退到CPU版本的compute_foe
-    """
-    # 检查CuPy是否可用
-    if not CUPY_AVAILABLE:
-        return compute_foe(flow_field, min_magnitude, expansion_threshold, downsample_factor, 
-                          return_all_contours, kernel_size, denoise_sigma)
-    
-    H, W = flow_field.shape[:2]
-    
-    # 导入CuPy的高斯滤波函数（仅在需要时导入）
-    try:
-        from cupyx.scipy.ndimage import gaussian_filter as cp_gaussian_filter
-    except ImportError:
-        # 如果导入失败，回退到CPU版本
-        return compute_foe(flow_field, min_magnitude, expansion_threshold, downsample_factor, 
-                          return_all_contours, kernel_size, denoise_sigma)
-    
-    # 优化：使用asarray而不是array，如果已经是CuPy数组则避免复制
-    if isinstance(flow_field, cp.ndarray):
-        flow_gpu = flow_field
-    else:
-        flow_gpu = cp.asarray(flow_field, dtype=cp.float32)
-    
-    # 优化：使用视图而不是复制
-    u_gpu = flow_gpu[:, :, 0]
-    v_gpu = flow_gpu[:, :, 1]
-    
-    # 下采样并计算散度，然后插值回原尺寸
-    if downsample_factor > 1:
-        # 下采样（使用步进切片，避免复制）
-        u_ds_gpu = u_gpu[::downsample_factor, ::downsample_factor]
-        v_ds_gpu = v_gpu[::downsample_factor, ::downsample_factor]
-        
-        # 计算幅度（优化：使用square减少中间数组）
-        magnitude_ds_gpu = cp.sqrt(cp.square(u_ds_gpu) + cp.square(v_ds_gpu))
-        
-        # 高斯滤波（使用CuPy的gaussian_filter）
-        if denoise_sigma > 0:
-            magnitude_ds_gpu = cp_gaussian_filter(magnitude_ds_gpu, sigma=denoise_sigma)
-        
-        # 优化：合并梯度计算，减少中间数组
-        # 计算散度：du/dx + dv/dy
-        du_dx_ds = cp.gradient(u_ds_gpu, axis=1)
-        dv_dy_ds = cp.gradient(v_ds_gpu, axis=0)
-        divergence_ds_gpu = du_dx_ds + dv_dy_ds
-        
-        # 对散度进行高斯滤波
-        if denoise_sigma > 0:
-            divergence_ds_gpu = cp_gaussian_filter(divergence_ds_gpu, sigma=denoise_sigma)
-        
-        # 优化：在GPU上完成插值，减少数据传输
-        # 使用CuPy的zoom进行插值（更高效）
-        from cupyx.scipy.ndimage import zoom as cp_zoom
-        zoom_factor = (H / divergence_ds_gpu.shape[0], W / divergence_ds_gpu.shape[1])
-        magnitude_gpu = cp_zoom(magnitude_ds_gpu, zoom_factor, order=3)  # 3 = cubic
-        divergence_gpu = cp_zoom(divergence_ds_gpu, zoom_factor, order=3)
-    else:
-        # 计算幅度（优化：使用square减少中间数组）
-        magnitude_gpu = cp.sqrt(cp.square(u_gpu) + cp.square(v_gpu))
-        
-        # 高斯滤波
-        if denoise_sigma > 0:
-            magnitude_gpu = cp_gaussian_filter(magnitude_gpu, sigma=denoise_sigma)
-        
-        # 优化：合并梯度计算
-        du_dx = cp.gradient(u_gpu, axis=1)
-        dv_dy = cp.gradient(v_gpu, axis=0)
-        divergence_gpu = du_dx + dv_dy
-        
-        # 对散度进行高斯滤波
-        if denoise_sigma > 0:
-            divergence_gpu = cp_gaussian_filter(divergence_gpu, sigma=denoise_sigma)
-    
-    # 优化：在GPU上完成掩码操作，减少数据传输
-    # CuPy的where不支持out参数，使用赋值代替
-    divergence_gpu = cp.where(divergence_gpu > min_magnitude, divergence_gpu, 0)
-    
-    # 在GPU上创建掩码
-    valid_mask_gpu = magnitude_gpu > min_magnitude
-    expansion_mask_gpu = (valid_mask_gpu & (divergence_gpu > expansion_threshold))
-    contraction_mask_gpu = (valid_mask_gpu & (divergence_gpu < -expansion_threshold))
-    
-    # 优化：一次性传输所有需要的数据回CPU
-    magnitude = cp.asnumpy(magnitude_gpu)
-    divergence = cp.asnumpy(divergence_gpu)
-    expansion_mask = cp.asnumpy(expansion_mask_gpu)
-    contraction_mask = cp.asnumpy(contraction_mask_gpu)
-    
-    # 初始化结果字典
-    result = _create_foe_result(expansion_mask, contraction_mask, divergence)
-    
-    # 寻找扩张区域轮廓（在CPU上进行，因为OpenCV的轮廓检测需要CPU）
-    # 优化：缓存形态学操作的核
-    kernel_key = kernel_size
-    if kernel_key not in _morphology_kernels:
-        _morphology_kernels[kernel_key] = np.ones(kernel_size, np.uint8)
-    
-    expansion_binary = expansion_mask.astype(np.uint8) * 255
-    if np.any(expansion_binary):
-        expansion_binary_closed = cv2.morphologyEx(expansion_binary, cv2.MORPH_CLOSE, _morphology_kernels[kernel_key])
-        contours, _ = cv2.findContours(expansion_binary_closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-        
-        if contours:
-            result['centroid_found'] = True
-            if return_all_contours:
-                result['_all_contours'] = contours
-            
-            largest_contour = max(contours, key=cv2.contourArea)
-            result['contour'] = largest_contour
-            
-            M = cv2.moments(largest_contour)
-            if M["m00"] > 0:
-                result['centroid'] = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
-    
-    return result
 
-def compute_multiple_foe(flow_field, min_magnitude=1.0, expansion_threshold=0.001, 
+    def _create_foe_result(self, expansion_mask, contraction_mask, divergence_map, 
-                         downsample_factor=1, min_contour_area=3000, denoise_sigma=15, kernel_size=(5,5)):
+                        centroid=None, centroid_found=False, contour=None, 
-    """
+                        all_contours=None):
-    计算光流场的多个扩张焦点(Foci of Expansion)。
+        """
-    
+        创建标准化的FOE结果字典，统一所有函数的返回格式。
-    Args:
-        flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
-        min_magnitude: 最小光流幅度阈值，低于此值的点不参与计算
-        expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
-        downsample_factor: 下采样因子，用于平滑计算（默认10）
-        min_contour_area: 最小轮廓面积阈值，小于此值的轮廓会被忽略
-        denoise_sigma: 降噪高斯滤波的标准差（默认1.5，设为0禁用降噪）
-        kernel_size: 形态学操作的核大小
         
-    Returns:
+        Args:
-        dict: 包含多个扩张焦点信息的字典，包括：
+            expansion_mask: 扩张区域掩码
-            - 'foe_points': 扩张焦点坐标列表 [(x1, y1), (x2, y2), ...]
+            contraction_mask: 收缩区域掩码
-            - 'expansion_regions': 扩张区域轮廓列表
+            divergence_map: 散度图
-            - 'expansion_mask': 扩张区域二值掩码
+            centroid: 质心坐标，可以是单个元组 (x, y) 或列表 [(x1, y1), (x2, y2), ...]
-            - 'contraction_mask': 收缩区域二值掩码
+            centroid_found: 是否找到质心
-            - 'divergence_map': 散度图
+            contour: 轮廓，可以是单个轮廓或轮廓列表
-    """
+            all_contours: 所有轮廓（仅在return_all_contours=True时使用）
-    # 调用 compute_foe 获取基础数据和所有轮廓
+        
-    foe_result = compute_foe_gpu(
+        Returns:
-        flow_field, 
+            dict: 标准化的FOE结果字典
-        min_magnitude=min_magnitude,
+        """
-        expansion_threshold=expansion_threshold,
+        result = {
-        downsample_factor=downsample_factor,
+            'expansion_mask': expansion_mask,
-        return_all_contours=True,
+            'contraction_mask': contraction_mask,
-        kernel_size=kernel_size,
+            'divergence_map': divergence_map,
-        denoise_sigma=denoise_sigma
+            'centroid': centroid if centroid is not None else [0,0],
-    )
+            'centroid_found': centroid_found,
+        }
+        
+        # 可选字段
+        if contour is not None:
+            result['contour'] = contour
+        
+        if all_contours is not None:
+            result['_all_contours'] = all_contours
+        
+        return result
     
-    # 构建多焦点结果，复用基础数据
+    def compute_foe(self, flow_field, return_all_contours=False):
-    result = _create_foe_result(
+    # def compute_foe(self, flow_field, min_magnitude=1.0, expansion_threshold=1, downsample_factor=1, return_all_contours=False, kernel_size=(5,5), denoise_sigma=10):
-        expansion_mask=foe_result['expansion_mask'],
+        """
-        contraction_mask=foe_result['contraction_mask'],
+        计算光流场的扩张焦点(Focus of Expansion)。
-        divergence_map=foe_result['divergence_map'],
+        
-        centroid_found=foe_result['centroid_found']
+        Args:
-    )
+            flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
-    
+            min_magnitude: 最小光流幅度阈值，低于此值的点不参与计算
-    # 处理所有轮廓，过滤并计算焦点
+            expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
-    if '_all_contours' in foe_result:
+            downsample_factor: 下采样因子，用于平滑计算（默认10）
-        for contour in foe_result['_all_contours']:
+            return_all_contours: 是否返回所有轮廓（默认False，仅返回最大轮廓）
-            # 过滤掉太小的轮廓
+            kernel_size: 形态学操作的核大小
-            if cv2.contourArea(contour) >= min_contour_area:
+            denoise_sigma: 降噪高斯滤波的标准差（默认1.5，设为0禁用降噪）
-                # 计算轮廓的质心作为焦点
+            
-                M = cv2.moments(contour)
+        Returns:
+            dict: 包含扩张焦点信息的字典
+        """
+        H, W = flow_field.shape[:2]
+        
+        # 优化：使用视图而不是复制，减少内存分配
+        u = flow_field[:, :, 0]
+        v = flow_field[:, :, 1]
+        
+        # 下采样并计算散度，然后插值回原尺寸
+        if self.downsample_factor > 1:
+            # 使用步进切片，避免复制
+            u_ds = u[::self.downsample_factor, ::self.downsample_factor]
+            v_ds = v[::self.downsample_factor, ::self.downsample_factor]
+            
+            # 计算幅度（优化：使用square减少中间数组）
+            magnitude_ds = np.sqrt(np.square(u_ds) + np.square(v_ds))
+            if self.denoise_sigma > 0:
+                magnitude_ds = cv2.GaussianBlur(magnitude_ds, (0, 0), self.denoise_sigma)
+            
+            # 优化：合并梯度计算，减少中间数组
+            # 计算散度：du/dx + dv/dy
+            du_dx = np.gradient(u_ds, axis=1)
+            dv_dy = np.gradient(v_ds, axis=0)
+            divergence_ds = du_dx + dv_dy  # 直接相加，避免中间数组
+            
+            if self.denoise_sigma > 0:
+                divergence_ds = cv2.GaussianBlur(divergence_ds, (0, 0), self.denoise_sigma)
+            
+            # 插值回原尺寸
+            magnitude = cv2.resize(magnitude_ds, (W, H), interpolation=cv2.INTER_CUBIC)
+            divergence = cv2.resize(divergence_ds, (W, H), interpolation=cv2.INTER_CUBIC)
+        else:
+            # 计算幅度（优化：使用square减少中间数组）
+            magnitude = np.sqrt(np.square(u) + np.square(v))
+            if self.denoise_sigma > 0:
+                magnitude = cv2.GaussianBlur(magnitude, (0, 0), self.denoise_sigma)
+            
+            # 优化：合并梯度计算
+            du_dx = np.gradient(u, axis=1)
+            dv_dy = np.gradient(v, axis=0)
+            divergence = du_dx + dv_dy
+            
+            # 降噪：在计算散度之后对散度图进行平滑
+            if self.denoise_sigma > 0:
+                divergence = cv2.GaussianBlur(divergence, (0, 0), self.denoise_sigma)
+        
+        # 优化：使用原地操作 - 只保留大于阈值的散度值，其他设为0
+        divergence = np.where(divergence > self.divergence_threshold, divergence, 0)
+        
+        # 创建掩码（使用原地操作优化）
+        valid_mask = magnitude > self.flow_threshold
+        expansion_mask = (valid_mask & (divergence > self.expansion_threshold))
+        contraction_mask = (valid_mask & (divergence < -self.expansion_threshold))
+        
+        # 初始化结果字典
+        result = self._create_foe_result(expansion_mask, contraction_mask, divergence)
+        
+        # 寻找扩张区域轮廓
+        # 优化：缓存形态学操作的核
+        # Convert kernel_size to tuple for use as dictionary key (lists are not hashable)
+        kernel_key = tuple(self.kernel_size) if isinstance(self.kernel_size, (list, np.ndarray)) else self.kernel_size
+        if kernel_key not in _morphology_kernels:
+            _morphology_kernels[kernel_key] = np.ones(self.kernel_size, np.uint8)
+        
+        expansion_binary = expansion_mask.astype(np.uint8) * 255
+        if np.any(expansion_binary):
+            expansion_binary_closed = cv2.morphologyEx(expansion_binary, cv2.MORPH_CLOSE, _morphology_kernels[kernel_key])
+            contours, _ = cv2.findContours(expansion_binary_closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+            
+            if contours:
+                result['centroid_found'] = True
+                if return_all_contours:
+                    result['_all_contours'] = contours
+                
+                largest_contour = max(contours, key=cv2.contourArea)
+                result['contour'] = largest_contour
+                
+                M = cv2.moments(largest_contour)
                 if M["m00"] > 0:
-                    cx = int(M["m10"] / M["m00"])
+                    result['centroid'] = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
-                    cy = int(M["m01"] / M["m00"])
+        
-                    result['centroid'].append((cx, cy))
+        self.foe_results = result
-                    if 'contour' not in result:
+        return self.foe_results
-                        result['contour'] = []
+
-                    result['contour'].append(contour)
+    def compute_foe_gpu(self, flow_field, return_all_contours=False):
-    
+        """
-    return result
+        使用CUDA加速计算光流场的扩张焦点(Focus of Expansion)。
+        
+        Args:
+            flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
+            flow_threshold: 最小光流幅度阈值，低于此值的点不参与计算
+            expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
+            downsample_factor: 下采样因子，用于平滑计算（默认10）
+            return_all_contours: 是否返回所有轮廓（默认False，仅返回最大轮廓）
+            kernel_size: 形态学操作的核大小
+            denoise_sigma: 降噪高斯滤波的标准差（默认1.5，设为0禁用降噪）
+            
+        Returns:
+            dict: 包含扩张焦点信息的字典
+            
+        Note:
+            如果CuPy不可用，会自动回退到CPU版本的compute_foe
+        """
+        # 检查CuPy是否可用
+        if not CUPY_AVAILABLE:
+            return self.compute_foe(flow_field, return_all_contours)
+        
+        H, W = flow_field.shape[:2]
+        
+        # 导入CuPy的高斯滤波函数（仅在需要时导入）
+        try:
+            from cupyx.scipy.ndimage import gaussian_filter as cp_gaussian_filter
+        except ImportError:
+            # 如果导入失败，回退到CPU版本
+            return self.compute_foe(flow_field, return_all_contours)
+        
+        # 优化：使用asarray而不是array，如果已经是CuPy数组则避免复制
+        if isinstance(flow_field, cp.ndarray):
+            flow_gpu = flow_field
+        else:
+            flow_gpu = cp.asarray(flow_field, dtype=cp.float32)
+        
+        # 优化：使用视图而不是复制
+        u_gpu = flow_gpu[:, :, 0]
+        v_gpu = flow_gpu[:, :, 1]
+        
+        # 下采样并计算散度，然后插值回原尺寸
+        if self.downsample_factor > 1:
+            # 下采样（使用步进切片，避免复制）
+            u_ds_gpu = u_gpu[::self.downsample_factor, ::self.downsample_factor]
+            v_ds_gpu = v_gpu[::self.downsample_factor, ::self.downsample_factor]
+            
+            # 计算幅度（优化：使用square减少中间数组）
+            magnitude_ds_gpu = cp.sqrt(cp.square(u_ds_gpu) + cp.square(v_ds_gpu))
+            
+            # 高斯滤波（使用CuPy的gaussian_filter）
+            if self.denoise_sigma > 0:
+                magnitude_ds_gpu = cp_gaussian_filter(magnitude_ds_gpu, sigma=self.denoise_sigma)
+            
+            # 优化：合并梯度计算，减少中间数组
+            # 计算散度：du/dx + dv/dy
+            du_dx_ds = cp.gradient(u_ds_gpu, axis=1)
+            dv_dy_ds = cp.gradient(v_ds_gpu, axis=0)
+            divergence_ds_gpu = du_dx_ds + dv_dy_ds
+            
+            # 对散度进行高斯滤波
+            if self.denoise_sigma > 0:
+                divergence_ds_gpu = cp_gaussian_filter(divergence_ds_gpu, sigma=self.denoise_sigma)
+            
+            # 优化：在GPU上完成插值，减少数据传输
+            # 使用CuPy的zoom进行插值（更高效）
+            from cupyx.scipy.ndimage import zoom as cp_zoom
+            zoom_factor = (H / divergence_ds_gpu.shape[0], W / divergence_ds_gpu.shape[1])
+            magnitude_gpu = cp_zoom(magnitude_ds_gpu, zoom_factor, order=3)  # 3 = cubic
+            divergence_gpu = cp_zoom(divergence_ds_gpu, zoom_factor, order=3)
+        else:
+            # 计算幅度（优化：使用square减少中间数组）
+            magnitude_gpu = cp.sqrt(cp.square(u_gpu) + cp.square(v_gpu))
+            
+            # 高斯滤波
+            if self.denoise_sigma > 0:
+                magnitude_gpu = cp_gaussian_filter(magnitude_gpu, sigma=self.denoise_sigma)
+            
+            # 优化：合并梯度计算
+            du_dx = cp.gradient(u_gpu, axis=1)
+            dv_dy = cp.gradient(v_gpu, axis=0)
+            divergence_gpu = du_dx + dv_dy
+            
+            # 对散度进行高斯滤波
+            if self.denoise_sigma > 0:
+                divergence_gpu = cp_gaussian_filter(divergence_gpu, sigma=self.denoise_sigma)
+        
+        # 优化：在GPU上完成掩码操作，减少数据传输
+        # 只保留大于阈值的散度值，其他设为0
+        divergence_gpu = cp.where(divergence_gpu > self.divergence_threshold, divergence_gpu, 0)
+        
+        # 在GPU上创建掩码
+        valid_mask_gpu = magnitude_gpu > self.flow_threshold
+        expansion_mask_gpu = (valid_mask_gpu & (divergence_gpu > self.expansion_threshold))
+        contraction_mask_gpu = (valid_mask_gpu & (divergence_gpu < -self.expansion_threshold))
+        
+        # 优化：一次性传输所有需要的数据回CPU
+        magnitude = cp.asnumpy(magnitude_gpu)
+        divergence = cp.asnumpy(divergence_gpu)
+        expansion_mask = cp.asnumpy(expansion_mask_gpu)
+        contraction_mask = cp.asnumpy(contraction_mask_gpu)
+        
+        # 初始化结果字典
+        result = self._create_foe_result(expansion_mask, contraction_mask, divergence)
+        
+        # 寻找扩张区域轮廓（在CPU上进行，因为OpenCV的轮廓检测需要CPU）
+        # 优化：缓存形态学操作的核
+        # Convert kernel_size to tuple for use as dictionary key (lists are not hashable)
+        kernel_key = tuple(self.kernel_size) if isinstance(self.kernel_size, (list, np.ndarray)) else self.kernel_size
+        if kernel_key not in _morphology_kernels:
+            _morphology_kernels[kernel_key] = np.ones(self.kernel_size, np.uint8)
+        
+        expansion_binary = expansion_mask.astype(np.uint8) * 255
+        if np.any(expansion_binary):
+            expansion_binary_closed = cv2.morphologyEx(expansion_binary, cv2.MORPH_CLOSE, _morphology_kernels[kernel_key])
+            contours, _ = cv2.findContours(expansion_binary_closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+            
+            if contours:
+                result['centroid_found'] = True
+                if return_all_contours:
+                    result['_all_contours'] = contours
+                
+                largest_contour = max(contours, key=cv2.contourArea)
+                result['contour'] = largest_contour
+                
+                M = cv2.moments(largest_contour)
+                if M["m00"] > 0:
+                    result['centroid'] = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
+        
+        self.foe_results = result
+        return self.foe_results
+
+    def compute_multiple_foe(self,flow_field):
+        """
+        计算光流场的多个扩张焦点(Foci of Expansion)。
+        
+        Args:
+            flow_field: 光流场，形状为 (H, W, 2) 的 numpy 数组
+            flow_threshold: 最小光流幅度阈值，低于此值的点不参与计算
+            expansion_threshold: 扩张判定阈值，用于区分扩张和收缩区域
+            downsample_factor: 下采样因子，用于平滑计算（默认10）
+            min_contour_area: 最小轮廓面积阈值，小于此值的轮廓会被忽略
+            denoise_sigma: 降噪高斯滤波的标准差（默认1.5，设为0禁用降噪）
+            kernel_size: 形态学操作的核大小
+            
+        Returns:
+            dict: 包含多个扩张焦点信息的字典，包括：
+                - 'foe_points': 扩张焦点坐标列表 [(x1, y1), (x2, y2), ...]
+                - 'expansion_regions': 扩张区域轮廓列表
+                - 'expansion_mask': 扩张区域二值掩码
+                - 'contraction_mask': 收缩区域二值掩码
+                - 'divergence_map': 散度图
+        """
+        # 调用 compute_foe_gpu 获取基础数据和所有轮廓
+        foe_result = self.compute_foe_gpu(flow_field, return_all_contours=True)
+        
+        # 构建多焦点结果，复用基础数据
+        result = self._create_foe_result(
+            expansion_mask=foe_result['expansion_mask'],
+            contraction_mask=foe_result['contraction_mask'],
+            divergence_map=foe_result['divergence_map'],
+            centroid_found=foe_result['centroid_found']
+        )
+        
+        # 处理所有轮廓，过滤并计算焦点
+        if '_all_contours' in foe_result:
+            for contour in foe_result['_all_contours']:
+                # 过滤掉太小的轮廓
+                if cv2.contourArea(contour) >= self.min_contour_area:
+                    # 计算轮廓的质心作为焦点
+                    M = cv2.moments(contour)
+                    if M["m00"] > 0:
+                        cx = int(M["m10"] / M["m00"])
+                        cy = int(M["m01"] / M["m00"])
+                        result['centroid'].append((cx, cy))
+                        if 'contour' not in result:
+                            result['contour'] = []
+                        result['contour'].append(contour)
+        
+        self.foe_results = result
+        return self.foe_results

src/orisys/sensor.py

@@ -20,7 +20,7 @@ from .tracker import Tracker
 from .nHHD import nHHD
 # from .getDensity import getFilteredDensity
 from . import configs 
-from .get_depth import *
+from .get_depth import Contact3D
 
 
 class Sensor:
@@ -132,8 +132,9 @@ class Sensor:
         
 
         # Initialize components
-        self._stitcher = Stitcher(isstitch=isstitch, index=self.config_index)
+        self._stitcher = Stitcher(isstitch=isstitch, index=self.config_index, config_stitcher=self.config_stitcher)
         self.tracker = Tracker(adaptive=True, cuda=cuda)
+        self.contact3d = Contact3D(config_contact=self.config_contact, verbose=self.verbose)
         self.hhd = None
         
         # Setup video capture
@@ -261,14 +262,15 @@ class Sensor:
     def compute_contact(self, multiple_foe=False, min_contour_area=3000, min_magnitude=1.0, expansion_threshold=0.01, downsample_factor=1, kernel_size=(5,5), denoise_sigma=15):
         
         if multiple_foe:
-            result = 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, denoise_sigma=denoise_sigma)
+            # result = 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, denoise_sigma=denoise_sigma)
-
+            result = self.contact3d.compute_multiple_foe(self.vnormal)
         else:
             if self.cuda: 
-                result = compute_foe_gpu(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
+                # result = compute_foe_gpu(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
-
+                result = self.contact3d.compute_foe_gpu(self.vnormal)
             else:
-                result = compute_foe(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
+                # result = compute_foe(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
+                result = self.contact3d.compute_foe(self.vnormal)
 
         self.centroid = result['centroid']
         self.centroid_found = result['centroid_found']
@@ -457,9 +459,8 @@ class Sensor:
         self.config_index = cam["index"]
         self.config_pos_default = cam["pos_default"]
 
-        config_stitcher = self.config["stitcher"]
+        self.config_stitcher = self.config["stitcher"]
-
+        self.config_contact = self.config["contact"]
-        config_contact = self.config["contact"]