unknown
184 lines
8.0 KiB
Python
184 lines
8.0 KiB
Python
"""
|
|
示例 1: 基础用法 - 单传感器同步处理
|
|
|
|
本示例演示 orisys 的基本使用流程：
|
|
1. 初始化传感器（视频文件或摄像头）
|
|
2. 循环获取图像并计算形变和触碰点
|
|
3. 读取各类传感器数据
|
|
4. 可视化显示
|
|
|
|
|
|
"""
|
|
import os
|
|
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
|
|
import cv2
|
|
import orisys
|
|
import numpy as np
|
|
from scipy.interpolate import griddata
|
|
|
|
def draw_arrows(image, optical_flow,fps=True, threshold=5, grid_spacing=40, arrow_scale=1.0):
|
|
"""
|
|
Draw colored optical flow arrows on sensor image based on flow magnitude.
|
|
|
|
Args:
|
|
threshold: Minimum flow magnitude to display arrow (default: 10.0)
|
|
grid_spacing: Spacing between arrows in pixels (default: 20)
|
|
arrow_scale: Scale factor for arrow length (default: 3.0)
|
|
|
|
Returns:
|
|
np.ndarray: 400x400 BGR image with colored arrows (JET colormap)
|
|
|
|
"""
|
|
# Create working copies to avoid modifying originals
|
|
|
|
# Convert grayscale to BGR to enable colored arrow drawing
|
|
if len(image.shape) == 2 or image.shape[2] == 1:
|
|
image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
|
|
|
|
# Calculate scaling factors between image and flow dimensions
|
|
img_height, img_width = image.shape[:2]
|
|
flow_height, flow_width = optical_flow.shape[:2]
|
|
scale_x, scale_y = img_width / flow_width, img_height / flow_height
|
|
|
|
# Create sampling grid for flow field (used for interpolation source points)
|
|
sample_step = 19 # Spacing between sample points
|
|
flow_start = np.stack(np.meshgrid(range(0, flow_width, sample_step), range(0, flow_height, sample_step)), 2)
|
|
flow_end = (optical_flow[flow_start[:, :, 1], flow_start[:, :, 0], :] * 2.0 + flow_start).astype(np.int32)
|
|
|
|
# Create regular grid for arrow placement in image coordinates
|
|
grid_x, grid_y = np.mgrid[grid_spacing//2:flow_height:grid_spacing, grid_spacing//2:flow_width:grid_spacing]
|
|
x_axis = (grid_y.reshape(-1) * scale_x).astype(np.int64) # Scale to image width
|
|
y_axis = (grid_x.reshape(-1) * scale_y).astype(np.int64) # Scale to image height
|
|
|
|
# Interpolate flow vectors from sample grid to regular arrow grid using cubic interpolation
|
|
grid_vector = griddata(
|
|
flow_start.reshape(-1, 2),
|
|
(flow_end - flow_start).reshape(-1, 2), # Flow from start to end (original to new position)
|
|
(grid_y, grid_x), method='cubic', fill_value=0
|
|
)
|
|
grid_vector[np.isnan(grid_vector)] = 0 # Replace NaN with zero for stability
|
|
grid_flat_scaled = grid_vector.reshape(-1, 2) * np.array([scale_x, scale_y])
|
|
|
|
# Compute arrow endpoints and ensure they stay within image bounds
|
|
end_x = (grid_flat_scaled[:, 0] / arrow_scale + x_axis).astype(np.int64)
|
|
end_y = (grid_flat_scaled[:, 1] / arrow_scale + y_axis).astype(np.int64)
|
|
valid_mask = (y_axis >= 0) & (y_axis < img_height) & (x_axis >= 0) & (x_axis < img_width)
|
|
valid_mask &= (end_x >= 0) & (end_x < img_width) & (end_y >= 0) & (end_y < img_height)
|
|
|
|
# Filter arrows by magnitude threshold and compute colors using JET colormap
|
|
magnitudes = np.linalg.norm(grid_flat_scaled, axis=1)
|
|
# Only show arrows where image is non-zero
|
|
# Check if image pixels at arrow positions are non-zero (sum across channels > 0)
|
|
# Ensure indices are within bounds before indexing
|
|
safe_y = np.clip(y_axis, 0, img_height - 1)
|
|
safe_x = np.clip(x_axis, 0, img_width - 1)
|
|
# Get pixel values at arrow positions and check if non-zero
|
|
pixel_values = image[safe_y, safe_x]
|
|
# When indexing 3D image with 1D arrays, result is 2D (N, channels)
|
|
# When indexing 2D image with 1D arrays, result is 1D (N,)
|
|
if pixel_values.ndim == 2: # BGR image (shape: N x 3)
|
|
pixel_sums = np.sum(pixel_values, axis=1)
|
|
else: # Grayscale image (shape: N,)
|
|
pixel_sums = pixel_values
|
|
valid_mask &= (pixel_sums > 0)
|
|
valid_indices = np.where(valid_mask)[0]
|
|
mag_range = magnitudes.max() - magnitudes.min()
|
|
# Normalize magnitudes to 0-255 range for colormap application, but set below-threshold values to 0 (blue)
|
|
norm_magnitudes = ((magnitudes - magnitudes.min()) / mag_range * 255.0).astype(np.uint8) if mag_range > 0 else np.zeros_like(magnitudes, dtype=np.uint8)
|
|
norm_magnitudes[magnitudes < threshold] = 0 # Keep lowest color (blue) for below-threshold magnitudes
|
|
colors_array = cv2.applyColorMap(norm_magnitudes.reshape(-1, 1), cv2.COLORMAP_BONE) # JET: blue (low) -> red (high)
|
|
|
|
# Draw arrows at each valid grid point with magnitude-based coloring
|
|
for i in valid_indices:
|
|
color = tuple(map(int, colors_array[i, 0]))
|
|
# Scale arrow length and thickness based on threshold
|
|
if magnitudes[i] < threshold:
|
|
# Below threshold: draw as small dot (zero-length arrow with larger tip)
|
|
cv2.circle(image, (x_axis[i], y_axis[i]), radius=1, color=color, thickness=-1)
|
|
else:
|
|
# Above threshold: draw full arrow with thickness proportional to magnitude
|
|
# Arrow rooted at original position (x_axis, y_axis), head points to new position (end_x, end_y)
|
|
thickness = max(1, int(2 * magnitudes[i] / magnitudes.max())) if magnitudes.max() > 0 else 2
|
|
cv2.arrowedLine(image, (x_axis[i], y_axis[i]), (end_x[i], end_y[i]), color, thickness=thickness, tipLength=0.3)
|
|
return image
|
|
|
|
def main():
|
|
|
|
# 步骤1：创建传感器对象 (必要步骤)，config_name为配置文件名（内置配置文件名）或路径（自定义配置文件路径）
|
|
|
|
# 方式1: 从视频文件读取
|
|
# sensor = orisys.Sensor("./s1.mp4", isstitch=True, cuda=True, verbose=True,config_name="prototype")
|
|
|
|
# 方式2: 从摄像头读取（取消注释使用）
|
|
sensor = orisys.Sensor(0, cuda=True, verbose=True, config_name="./config/demo.json")
|
|
sensor.export_config(path="./config/exported_config.json") # 导出配置文件供查看和修改
|
|
|
|
print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")
|
|
|
|
|
|
while True:
|
|
# 步骤2：信息提取
|
|
# 步骤 2.1: 获取并拼接图像 (必要步骤)
|
|
sensor.get_img()
|
|
|
|
# 步骤 2.2: 计算形变场 (必要步骤)
|
|
sensor.compute_deformation()
|
|
# 步骤 2.3: 计算触碰点
|
|
sensor.compute_contact()
|
|
|
|
#步骤 2.4: 读取信息
|
|
fps, flow, vnormal, img, contour, centroid, depth_map = sensor.read_info(
|
|
sensor.info.FPS,
|
|
sensor.info.VRAW,
|
|
sensor.info.VNORMAL,
|
|
sensor.info.IMG,
|
|
sensor.info.CONTOUR,
|
|
sensor.info.CENTROID,
|
|
sensor.info.DEPTH
|
|
)
|
|
|
|
#步骤 3: 可视化显示
|
|
# 步骤 3.3: 绘制深度图
|
|
# 归一化散度值到0-255范围（使用绝对值，然后归一化）
|
|
div_abs = depth_map
|
|
if div_abs.max() > div_abs.min():
|
|
div_normalized = ((div_abs - div_abs.min()) / (div_abs.max() - div_abs.min()) * 255).astype(np.uint8)
|
|
else:
|
|
div_normalized = np.zeros_like(div_abs, dtype=np.uint8)
|
|
|
|
# 应用颜色映射（使用JET：蓝色=低散度，红色=高散度）
|
|
img_depth = cv2.applyColorMap(div_normalized, cv2.COLORMAP_HOT)
|
|
|
|
arrows = draw_arrows(
|
|
img_depth,
|
|
flow,
|
|
threshold=1,
|
|
grid_spacing=10,
|
|
arrow_scale=0.5
|
|
)
|
|
cv2.imshow("形变场 Deformation", arrows)
|
|
|
|
|
|
|
|
#步骤 3.4: 打印资料
|
|
print(f"fps: {fps}")
|
|
|
|
#步骤 3.5: 键盘控制
|
|
key = cv2.waitKey(1) & 0xFF
|
|
if key == ord("q"):
|
|
print("\n退出程序...")
|
|
break
|
|
if key == ord("r"):
|
|
# 重置光流追踪器（当追踪漂移时使用）
|
|
sensor.reset()
|
|
print("光流追踪器已重置")
|
|
|
|
# =========================================================================
|
|
# 4. 清理资源
|
|
# =========================================================================
|
|
sensor.disconnect() # 释放视频源和相关资源
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|