From 017561578dcaebf09ee99d964a4edaae23f27abc Mon Sep 17 00:00:00 2001 From: bluepulsar Date: Sun, 28 Dec 2025 12:20:44 +0800 Subject: [PATCH] add focus of expansion (foe) detection --- src/orisys/get_depth.py | 154 ++++++++++++++++++++++++++++++++++++++++ src/orisys/sensor.py | 31 +++++--- src/orisys/util.py | 91 +++++++++++++++++++++++- 3 files changed, 263 insertions(+), 13 deletions(-) create mode 100644 src/orisys/get_depth.py diff --git a/src/orisys/get_depth.py b/src/orisys/get_depth.py new file mode 100644 index 0000000..1e076d1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index dfe335b..8025124 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -19,7 +19,8 @@ from .Stitcher import Stitcher from .tracker import Tracker from .nHHD import nHHD from .getDensity import getFilteredDensity -from . import configs +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], }) diff --git a/src/orisys/util.py b/src/orisys/util.py index 726df8c..8356736 100644 --- a/src/orisys/util.py +++ b/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 \ No newline at end of file