Merge branch 'remote'
This commit is contained in:
unknown
@@ -19,10 +19,10 @@ def main():
|
||||
# 步骤1：创建传感器对象 (必要步骤)，config_name为配置文件名（内置配置文件名）或路径（自定义配置文件路径）
|
||||
|
||||
# 方式1: 从视频文件读取
|
||||
# sensor = orisys.Sensor("./s1.mp4", isstitch=True, cuda=True, verbose=True,config_name="prototype")
|
||||
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="prototype")
|
||||
# sensor = orisys.Sensor(0, cuda=True, verbose=True, config_name="prototype")
|
||||
sensor.export_config(path="./config/exported_config.json") # 导出配置文件供查看和修改
|
||||
|
||||
print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")
|
||||
|
||||
@@ -1,115 +0,0 @@
|
||||
"""
|
||||
示例 5: 异步流水线处理（单传感器高吞吐模式）
|
||||
|
||||
本示例演示传感器的异步三阶段流水线：
|
||||
- 图像采集、光流计算、HHD分解并行执行
|
||||
- 通过队列在线程间传递数据
|
||||
- 实现更高的帧率和吞吐量
|
||||
|
||||
流水线架构：
|
||||
[采集线程] → 队列 → [光流线程] → 队列 → [HHD线程] → 队列 → [主线程显示]
|
||||
|
||||
时间线示例（每格代表一帧的处理时间）：
|
||||
采集: |帧1|帧2|帧3|帧4|帧5|...
|
||||
光流: |帧1|帧2|帧3|帧4|...
|
||||
HHD: |帧1|帧2|帧3|...
|
||||
|
||||
优势：三个阶段重叠执行，总吞吐量约为最慢阶段的速度
|
||||
|
||||
适用场景：
|
||||
- 单传感器需要最大帧率
|
||||
- CPU/GPU 利用率需要最大化
|
||||
- 可以接受 2-3 帧的处理延迟
|
||||
|
||||
对比同步模式：
|
||||
- 同步模式：采集→光流→HHD 串行执行，FPS 受限于总时间
|
||||
- 异步模式：三阶段并行，FPS 约为同步模式的 2-3 倍
|
||||
"""
|
||||
|
||||
import os
|
||||
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" # Windows 兼容性设置
|
||||
|
||||
import cv2
|
||||
import orisys
|
||||
import numpy as np
|
||||
|
||||
def main():
|
||||
# =========================================================================
|
||||
# 1. 初始化传感器
|
||||
# =========================================================================
|
||||
sensor = orisys.Sensor(0, cuda=True, verbose=True, config_name="prototype")
|
||||
|
||||
# =========================================================================
|
||||
# 2. 启用异步流水线
|
||||
# =========================================================================
|
||||
# queue_size: 每个阶段间队列的大小
|
||||
# - 较小的值（1-2）：延迟低，但可能丢帧
|
||||
# - 较大的值（3-5）：吞吐稳定，但延迟增加
|
||||
sensor.enable_async(queue_size=2, compute_contact=True)
|
||||
|
||||
print("\n异步模式已启用")
|
||||
print("按 'q' 退出，按 'r' 重置光流追踪器")
|
||||
|
||||
# =========================================================================
|
||||
# 3. 主循环：非阻塞获取数据
|
||||
# =========================================================================
|
||||
try:
|
||||
while True:
|
||||
# 非阻塞获取最新结果
|
||||
# timeout: 等待超时时间（秒）
|
||||
# - 返回 None 表示暂无新数据
|
||||
# - 返回 dict 包含所有传感器数据
|
||||
data = sensor.get_async(timeout=0.05)
|
||||
|
||||
if data is None:
|
||||
# 暂无新数据，继续等待
|
||||
continue
|
||||
|
||||
# 打印力的数值
|
||||
print(f"FPS: {data['fps']:5.1f}, "
|
||||
f"法向力: {data['fnormal']:8.2f}, "
|
||||
f"切向力X: {data['fshearx']:8.2f}, "
|
||||
f"切向力Y: {data['fsheary']:8.2f}")
|
||||
|
||||
# 显示形变箭头图
|
||||
if data['img'] is not None and data['flow'] is not None:
|
||||
arrows = orisys.util.draw_arrows(
|
||||
data['img'].copy(),
|
||||
data['flow'],
|
||||
threshold=5
|
||||
)
|
||||
cv2.imshow("异步模式 - 形变", arrows)
|
||||
|
||||
if data['centroid'] is not None:
|
||||
# print(f"触碰点: {data['centroid']}")
|
||||
image_with_foe = orisys.util.draw_contact(data['img'].copy(), data['contour'], data['centroid'])
|
||||
cv2.imshow("Contact Centroid", image_with_foe)
|
||||
|
||||
if data['depth_map'] is not None:
|
||||
# 归一化散度值到0-255范围（使用绝对值，然后归一化）
|
||||
div_abs = data['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)
|
||||
cv2.imshow("Depth Map", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
|
||||
|
||||
# 键盘控制
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord('q'):
|
||||
break
|
||||
elif key == ord('r'):
|
||||
sensor.reset()
|
||||
print("光流追踪器已重置")
|
||||
|
||||
finally:
|
||||
# =====================================================================
|
||||
# 4. 清理资源
|
||||
# =====================================================================
|
||||
# disconnect() 会自动调用 disable_async() 停止所有线程
|
||||
sensor.disconnect()
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,237 @@
|
||||
"""
|
||||
示例 4: 多传感器并行处理（多进程模式 - 不使用 MultiSensor）
|
||||
|
||||
本示例演示如何使用 multiprocessing 同时运行两个传感器：
|
||||
- 每个传感器运行在独立的进程中
|
||||
- 每个进程拥有独立的 CUDA 上下文
|
||||
- 使用 multiprocessing.Process 和 Queue 进行进程间通信
|
||||
|
||||
架构说明：
|
||||
进程 1 (传感器 0): 采集 → 光流 → HHD → 接触检测 → 队列
|
||||
进程 2 (传感器 1): 采集 → 光流 → HHD → 接触检测 → 队列
|
||||
↓
|
||||
主进程: 显示 ← ───────────────┘
|
||||
|
||||
适用场景：多指触觉传感器、多视角采集、需要 GPU 加速的多路处理
|
||||
|
||||
注意事项：
|
||||
- Windows 下必须在 if __name__ == '__main__' 中调用 mp.freeze_support()
|
||||
- 每个传感器进程会占用独立的 GPU 显存
|
||||
"""
|
||||
import os
|
||||
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
|
||||
import cv2
|
||||
import orisys
|
||||
import numpy as np
|
||||
import multiprocessing as mp
|
||||
from multiprocessing import Queue, Process
|
||||
import time
|
||||
|
||||
|
||||
def sensor_worker(sensor_id, vid_src, config_name, result_queue, stop_event):
|
||||
"""
|
||||
传感器工作进程函数
|
||||
|
||||
Args:
|
||||
sensor_id: 传感器编号
|
||||
vid_src: 视频源（文件路径或摄像头索引）
|
||||
config_name: 配置名称
|
||||
result_queue: 结果队列，用于向主进程发送数据
|
||||
stop_event: 停止事件，用于通知工作进程退出
|
||||
"""
|
||||
try:
|
||||
# 在每个进程中创建独立的传感器实例
|
||||
sensor = orisys.Sensor(
|
||||
vid_src,
|
||||
isstitch=True,
|
||||
cuda=True,
|
||||
verbose=False, # 减少子进程输出
|
||||
config_name=config_name
|
||||
)
|
||||
|
||||
print(f"传感器 {sensor_id} 已启动")
|
||||
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
# 步骤1: 获取图像
|
||||
sensor.get_img()
|
||||
|
||||
# 步骤2: 计算形变场
|
||||
sensor.compute_deformation()
|
||||
|
||||
# 步骤3: 计算触碰点
|
||||
sensor.compute_contact()
|
||||
|
||||
# 步骤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
|
||||
)
|
||||
|
||||
# 步骤5: 准备数据字典（确保所有数据都是可序列化的）
|
||||
data = {
|
||||
'sensor_id': sensor_id,
|
||||
'fps': fps,
|
||||
'flow': flow.copy() if flow is not None else None,
|
||||
'vnormal': vnormal.copy() if vnormal is not None else None,
|
||||
'img': img.copy() if img is not None else None,
|
||||
'contour': contour,
|
||||
'centroid': centroid,
|
||||
'depth_map': depth_map.copy() if depth_map is not None else None,
|
||||
'timestamp': time.time()
|
||||
}
|
||||
|
||||
# 步骤6: 发送数据到主进程（非阻塞）
|
||||
try:
|
||||
result_queue.put_nowait(data)
|
||||
except:
|
||||
# 如果队列已满，跳过这一帧
|
||||
pass
|
||||
|
||||
# 短暂休眠，避免过度占用CPU
|
||||
time.sleep(0.001)
|
||||
|
||||
except Exception as e:
|
||||
print(f"传感器 {sensor_id} 处理错误: {e}")
|
||||
time.sleep(0.1)
|
||||
|
||||
# 清理资源
|
||||
sensor.disconnect()
|
||||
print(f"传感器 {sensor_id} 已停止")
|
||||
|
||||
except Exception as e:
|
||||
print(f"传感器 {sensor_id} 初始化失败: {e}")
|
||||
result_queue.put({'sensor_id': sensor_id, 'error': str(e)})
|
||||
|
||||
|
||||
def main():
|
||||
# =========================================================================
|
||||
# 1. 配置多个传感器
|
||||
# =========================================================================
|
||||
num_sensors = 2
|
||||
sensor_configs = [
|
||||
{
|
||||
'vid_src': './s1.mp4',
|
||||
'config_name': 'prototype'
|
||||
},
|
||||
{
|
||||
'vid_src': './s1.mp4', # 可以改为不同的视频源或摄像头索引
|
||||
'config_name': 'prototype'
|
||||
},
|
||||
]
|
||||
|
||||
# =========================================================================
|
||||
# 2. 创建进程间通信对象
|
||||
# =========================================================================
|
||||
result_queues = [Queue(maxsize=num_sensors) for _ in sensor_configs] # 每个传感器一个队列
|
||||
stop_events = [mp.Event() for _ in sensor_configs] # 停止事件
|
||||
|
||||
# =========================================================================
|
||||
# 3. 启动传感器工作进程
|
||||
# =========================================================================
|
||||
processes = []
|
||||
for i, config in enumerate(sensor_configs):
|
||||
p = Process(
|
||||
target=sensor_worker,
|
||||
args=(i, config['vid_src'], config['config_name'], result_queues[i], stop_events[i])
|
||||
)
|
||||
p.start()
|
||||
processes.append(p)
|
||||
|
||||
print(f"\n已启动 {len(processes)} 个传感器进程")
|
||||
print("按 'q' 退出程序")
|
||||
|
||||
# 用于存储每个传感器的最新数据
|
||||
latest_data = [None] * len(sensor_configs)
|
||||
|
||||
try:
|
||||
# =====================================================================
|
||||
# 4. 主循环：从队列获取数据并显示
|
||||
# =====================================================================
|
||||
while True:
|
||||
# 从所有队列中获取最新数据
|
||||
for i, queue in enumerate(result_queues):
|
||||
try:
|
||||
# 非阻塞获取，获取队列中最新的数据
|
||||
while True:
|
||||
data = queue.get_nowait()
|
||||
latest_data[i] = data
|
||||
except:
|
||||
# 队列为空，使用之前的数据
|
||||
pass
|
||||
|
||||
# 显示所有传感器的数据
|
||||
for data in latest_data:
|
||||
if data is None or 'error' in data:
|
||||
continue
|
||||
|
||||
sid = data['sensor_id']
|
||||
|
||||
# 显示形变场
|
||||
if data['img'] is not None and data['flow'] is not None:
|
||||
arrows = orisys.util.draw_arrows(
|
||||
data['img'],
|
||||
data['flow'],
|
||||
threshold=5,
|
||||
grid_spacing=10,
|
||||
arrow_scale=1.0
|
||||
)
|
||||
cv2.imshow(f"形变场 Deformation {sid}", arrows)
|
||||
|
||||
# 显示触碰点和区域
|
||||
if data['img'] is not None:
|
||||
image_with_foe = orisys.util.draw_contact(
|
||||
data['img'],
|
||||
data['contour'],
|
||||
data['centroid']
|
||||
)
|
||||
cv2.imshow(f"Contact Centroid {sid}", image_with_foe)
|
||||
|
||||
# 显示深度图
|
||||
if data['depth_map'] is not None and data['depth_map'].size > 0:
|
||||
div_abs = data['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)
|
||||
cv2.imshow(f"Depth Map {sid}",
|
||||
cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
|
||||
|
||||
# 打印FPS
|
||||
print(f"传感器 {sid}: FPS={data['fps']:5.1f}")
|
||||
|
||||
# 检查退出键
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord('q'):
|
||||
print("\n退出程序...")
|
||||
break
|
||||
|
||||
finally:
|
||||
# =====================================================================
|
||||
# 5. 清理资源
|
||||
# =====================================================================
|
||||
# 通知所有工作进程停止
|
||||
for event in stop_events:
|
||||
event.set()
|
||||
|
||||
# 等待所有进程结束
|
||||
for p in processes:
|
||||
p.join(timeout=2)
|
||||
if p.is_alive():
|
||||
p.terminate()
|
||||
p.join()
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
print("所有传感器进程已停止")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Windows 下多进程必需的初始化
|
||||
mp.freeze_support()
|
||||
main()
|
||||
@@ -1,118 +0,0 @@
|
||||
"""
|
||||
示例 4: 多传感器并行处理（多进程模式）
|
||||
|
||||
本示例演示如何同时运行多个传感器：
|
||||
- 每个传感器运行在独立的进程中
|
||||
- 每个进程拥有独立的 CUDA 上下文
|
||||
- 使用 MultiSensor 类简化多传感器管理
|
||||
|
||||
架构说明：
|
||||
进程 1 (传感器 0): 采集 → 光流 → HHD → 队列
|
||||
进程 2 (传感器 1): 采集 → 光流 → HHD → 队列
|
||||
↓
|
||||
主进程: 显示 ← ───────────────┘
|
||||
|
||||
适用场景：多指触觉传感器、多视角采集、需要 GPU 加速的多路处理
|
||||
|
||||
注意事项：
|
||||
- Windows 下必须在 if __name__ == '__main__' 中调用 mp.freeze_support()
|
||||
- 每个传感器进程会占用独立的 GPU 显存
|
||||
"""
|
||||
|
||||
import os
|
||||
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" # Windows 兼容性设置
|
||||
|
||||
import cv2
|
||||
import orisys
|
||||
import multiprocessing as mp
|
||||
import numpy as np
|
||||
|
||||
|
||||
def main():
|
||||
# =========================================================================
|
||||
# 1. 配置多个传感器
|
||||
# =========================================================================
|
||||
# 每个传感器的配置是一个字典，包含：
|
||||
# vid_src: 视频源（文件路径或摄像头索引）
|
||||
# isstitch: 是否启用拼接模式
|
||||
# cuda: 是否使用 GPU 加速
|
||||
# cal_path: 标定文件路径
|
||||
configs = [
|
||||
{
|
||||
'vid_src': './s1.mp4',
|
||||
'isstitch': True,
|
||||
'cuda': True,
|
||||
'cal_path': './config/cal.npy',
|
||||
'config_name': "prototype"
|
||||
},
|
||||
{
|
||||
'vid_src': './s1.mp4',
|
||||
'isstitch': True,
|
||||
'cuda': True,
|
||||
'cal_path': './config/cal.npy',
|
||||
'config_name': "prototype"
|
||||
},
|
||||
]
|
||||
|
||||
# =========================================================================
|
||||
# 2. 使用上下文管理器启动多传感器
|
||||
# =========================================================================
|
||||
# MultiSensor 会自动：
|
||||
# - 为每个配置创建独立的子进程
|
||||
# - 在子进程中初始化 Sensor 实例
|
||||
# - 持续采集和处理数据
|
||||
# - 通过队列将结果传回主进程
|
||||
# compute_contact=True 启用接触点检测（深度图、质心、轮廓）
|
||||
with orisys.MultiSensor(configs, compute_contact=True) as multi:
|
||||
print("\n多传感器已启动，按 'q' 退出")
|
||||
|
||||
# =====================================================================
|
||||
# 3. 迭代获取所有传感器的数据
|
||||
# =====================================================================
|
||||
# multi 是可迭代的，每次迭代返回所有传感器的最新数据列表
|
||||
# all_data = [data_sensor_0, data_sensor_1, ...]
|
||||
for all_data in multi:
|
||||
# 过滤掉 None（某些传感器可能暂时没有新数据）
|
||||
for data in [d for d in all_data if d]:
|
||||
sid = data['sensor_id'] # 传感器编号
|
||||
|
||||
# 打印传感器状态
|
||||
print(f"传感器 {sid}: FPS={data['fps']:5.1f}, 法向力={data['fnormal']:8.2f}")
|
||||
|
||||
# 显示形变箭头图
|
||||
if data['img'] is not None and data['flow'] is not None:
|
||||
arrows = orisys.util.draw_arrows(
|
||||
data['img'].copy(),
|
||||
data['flow'],
|
||||
threshold=5
|
||||
)
|
||||
cv2.imshow(f"Deformation {sid}", arrows)
|
||||
|
||||
|
||||
if data['centroid'] is not None:
|
||||
# print(f"触碰点: {data['centroid']}")
|
||||
image_with_foe = orisys.util.draw_contact(data['img'].copy(), data['contour'], data['centroid'])
|
||||
cv2.imshow(f"Contact Centroid {sid}", image_with_foe)
|
||||
|
||||
if data['depth_map'] is not None and data['depth_map'].size > 0:
|
||||
# 归一化散度值到0-255范围（使用绝对值，然后归一化）
|
||||
div_abs = data['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)
|
||||
cv2.imshow(f"Depth Map {sid}", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
|
||||
|
||||
|
||||
# 检查退出键
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
break
|
||||
|
||||
# 上下文管理器退出时自动停止所有子进程
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Windows 下多进程必需的初始化
|
||||
mp.freeze_support()
|
||||
main()
|
||||
@@ -9,7 +9,7 @@ independent CUDA contexts.
|
||||
import os
|
||||
|
||||
|
||||
def _sensor_worker(sensor_id, config, data_queue, stop_event, compute_contact=False):
|
||||
def _sensor_worker(sensor_id, config, data_queue, stop_event):
|
||||
"""
|
||||
Worker function that runs in a separate PROCESS for each sensor.
|
||||
Each process has its own Python interpreter and CUDA context.
|
||||
@@ -28,6 +28,7 @@ def _sensor_worker(sensor_id, config, data_queue, stop_event, compute_contact=Fa
|
||||
config_name=config.get('config_name', 'prototype'),
|
||||
cal_path=config.get('cal_path', None)
|
||||
)
|
||||
compute_contact = config.get('compute_contact', False)
|
||||
|
||||
frame_count = 0
|
||||
|
||||
@@ -156,7 +157,7 @@ class MultiSensor:
|
||||
multi.stop()
|
||||
"""
|
||||
|
||||
def __init__(self, sensor_configs, compute_contact=False):
|
||||
def __init__(self, sensor_configs):
|
||||
"""
|
||||
Initialize MultiSensor manager.
|
||||
|
||||
@@ -181,7 +182,6 @@ class MultiSensor:
|
||||
self.sensor_configs = sensor_configs
|
||||
self.num_sensors = len(sensor_configs)
|
||||
self._mp = mp
|
||||
self.compute_contact = compute_contact
|
||||
|
||||
self.processes = []
|
||||
self.data_queues = []
|
||||
@@ -202,7 +202,7 @@ class MultiSensor:
|
||||
for i, config in enumerate(self.sensor_configs):
|
||||
process = self._mp.Process(
|
||||
target=_sensor_worker,
|
||||
args=(i, config, self.data_queues[i], self.stop_events[i], self.compute_contact),
|
||||
args=(i, config, self.data_queues[i], self.stop_events[i]),
|
||||
daemon=True
|
||||
)
|
||||
process.start()
|
||||
|
||||
@@ -56,6 +56,17 @@ def load_config(config_name_or_path):
|
||||
with open(package_config_path, 'r', encoding='utf-8') as f:
|
||||
return json.load(f)
|
||||
|
||||
def list_configs():
|
||||
"""
|
||||
List all available JSON configuration files in the configs directory.
|
||||
|
||||
Returns:
|
||||
List of configuration names (without .json extension), sorted alphabetically.
|
||||
Returns an empty list if no JSON files are found.
|
||||
"""
|
||||
config_files = sorted(CONFIG_DIR.glob("*.json"))
|
||||
return [f.stem for f in config_files]
|
||||
|
||||
def _format_json_value(value, indent_level=0, indent_str=" "):
|
||||
"""
|
||||
Recursively format JSON value with arrays/lists on single line.
|
||||
|
||||
+197
-211
@@ -292,26 +292,6 @@ class Sensor:
|
||||
return result
|
||||
|
||||
|
||||
# def get_depth(self, denoise_sigma=15):
|
||||
# return getFilteredDensity(self.vnormal, use_cuda=self.cuda)
|
||||
# self.depth = getFilteredDensity(self.vnormal, use_cuda=self.cuda)
|
||||
|
||||
def update(self):
|
||||
"""
|
||||
Convenience method: get_img + compute_deformation in one call.
|
||||
|
||||
Returns:
|
||||
dict: All sensor data
|
||||
"""
|
||||
self.get_img()
|
||||
self.compute_deformation()
|
||||
return {
|
||||
'img': self.__img, 'flow': self.__flow,
|
||||
'normal': self.__vnormal, 'shear': self.__vshear, 'depth': self.__depth_map,
|
||||
'fps': self._fps, 'fnormal': self.__fnormal,
|
||||
'fshearx': self.__fshearx, 'fsheary': self.__fsheary, 'ftor': self.__ftor,
|
||||
}
|
||||
|
||||
def read_info(self, *args) -> Tuple:
|
||||
"""
|
||||
Get sensor data by info type.
|
||||
@@ -374,88 +354,6 @@ class Sensor:
|
||||
print("Sensor disconnected.")
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# =========================================================================
|
||||
# Public API - Async Pipeline
|
||||
# =========================================================================
|
||||
|
||||
def enable_async(self, queue_size=2, compute_contact=False):
|
||||
"""
|
||||
Enable 3-stage async pipeline: Capture → Flow → HHD.
|
||||
|
||||
Each stage runs in parallel for maximum throughput.
|
||||
"""
|
||||
if self._async_enabled:
|
||||
return
|
||||
self._compute_contact_enabled = compute_contact
|
||||
|
||||
self._capture_queue = Queue(maxsize=queue_size)
|
||||
self._flow_queue = Queue(maxsize=queue_size)
|
||||
self._result_queue = Queue(maxsize=queue_size)
|
||||
if compute_contact:
|
||||
self._contact_queue = Queue(maxsize=queue_size)
|
||||
self._stop_event = Event()
|
||||
self._async_enabled = True
|
||||
|
||||
self._capture_thread = Thread(target=self._capture_worker, daemon=True)
|
||||
self._flow_thread = Thread(target=self._flow_worker, daemon=True)
|
||||
self._compute_thread = Thread(target=self._hhd_worker, daemon=True)
|
||||
if compute_contact:
|
||||
self._contact_thread = Thread(target=self._contact_worker, daemon=True)
|
||||
|
||||
self._capture_thread.start()
|
||||
self._flow_thread.start()
|
||||
self._compute_thread.start()
|
||||
if compute_contact:
|
||||
self._contact_thread.start()
|
||||
|
||||
if self.verbose:
|
||||
if compute_contact:
|
||||
print("Async pipeline enabled: Capture → Flow → HHD → Contact")
|
||||
else:
|
||||
print("Async pipeline enabled: Capture → Flow → HHD")
|
||||
|
||||
def get_async(self, timeout=0.05):
|
||||
"""
|
||||
Get latest result from async pipeline (non-blocking).
|
||||
|
||||
Returns:
|
||||
dict or None: Sensor data if available
|
||||
"""
|
||||
compute_contact = self._compute_contact_enabled
|
||||
if not self._async_enabled:
|
||||
return None
|
||||
try:
|
||||
result = self._result_queue.get(timeout=timeout)
|
||||
t = time.perf_counter()
|
||||
result['fps'] = 1.0 / (t - self.last_log_time) if self.last_log_time else 0
|
||||
self.last_log_time = t
|
||||
|
||||
# Sync instance variables
|
||||
# for k in ['img', 'flow', 'normal', 'shear', 'depth', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
if compute_contact:
|
||||
for k in ['img', 'flow', 'normal', 'shear', 'depth', 'centroid', 'centroid_found', 'contour', 'fps', 'fnormal', 'fshearx', 'fsheary', 'ftor']:
|
||||
setattr(self, k, result.get(k))
|
||||
else:
|
||||
for k in ['img', 'flow', 'normal', 'shear', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
setattr(self, k, result.get(k))
|
||||
self.vupdate = True
|
||||
return result
|
||||
except Empty:
|
||||
return None
|
||||
|
||||
def disable_async(self):
|
||||
"""Stop async pipeline."""
|
||||
if not self._async_enabled:
|
||||
return
|
||||
self._stop_event.set()
|
||||
for t in [self._capture_thread, self._flow_thread, self._compute_thread]:
|
||||
if t: t.join(timeout=2.0)
|
||||
self._async_enabled = False
|
||||
self._capture_thread = self._flow_thread = self._compute_thread = None
|
||||
self._capture_queue = self._flow_queue = self._result_queue = self._stop_event = None
|
||||
if self.verbose:
|
||||
print("Async pipeline disabled")
|
||||
|
||||
# =========================================================================
|
||||
# Public API - Configuration & Calibration
|
||||
# =========================================================================
|
||||
@@ -478,16 +376,19 @@ class Sensor:
|
||||
self.cal_path = "./config/cal.npy"
|
||||
|
||||
if os.path.exists(self.cal_path):
|
||||
print("Loading calibration from", self.cal_path)
|
||||
if self.verbose:
|
||||
print("Loading calibration from", self.cal_path)
|
||||
self.stitch_calibration = np.load(self.cal_path, allow_pickle=True)
|
||||
self.__stitcher.stitch_calibration = self.stitch_calibration
|
||||
return self.__stitcher.stitch_rt_optimized()
|
||||
else:
|
||||
print("Creating new calibration...")
|
||||
if self.verbose:
|
||||
print("Creating new calibration...")
|
||||
img, self.stitch_calibration = self.__stitcher.stitch_cal()
|
||||
os.makedirs("./config", exist_ok=True)
|
||||
np.save(self.cal_path, self.stitch_calibration)
|
||||
print("Calibration saved to", self.cal_path)
|
||||
if self.verbose:
|
||||
print("Calibration saved to", self.cal_path)
|
||||
return img
|
||||
|
||||
# =========================================================================
|
||||
@@ -508,8 +409,6 @@ class Sensor:
|
||||
|
||||
if self.verbose:
|
||||
print(f"Config: {self.config_name}")
|
||||
print(f" Resolution: {self._config_resolution_raw} → {self._config_resolution_stitch}")
|
||||
print(f" FPS: {self._config_fps}, Units: {self._config_units}")
|
||||
return True
|
||||
|
||||
def _open_video_source(self, override, box):
|
||||
@@ -588,110 +487,6 @@ class Sensor:
|
||||
self.__stitcher.extract_tiles()
|
||||
return True
|
||||
|
||||
# =========================================================================
|
||||
# Private - Async Workers
|
||||
# =========================================================================
|
||||
|
||||
def _queue_put(self, q, item):
|
||||
"""Put item in queue, dropping old if full."""
|
||||
if q.full():
|
||||
try: q.get_nowait()
|
||||
except Empty: pass
|
||||
q.put(item)
|
||||
|
||||
def _capture_worker(self):
|
||||
"""Stage 1: Capture and preprocess frames by reusing get_img()."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
# Reuse get_img() to avoid code duplication
|
||||
if self.get_img() is None:
|
||||
continue
|
||||
|
||||
# Put the processed image in the queue
|
||||
# get_img() updates self.__img, so we use that
|
||||
if self.__img is not None:
|
||||
self._queue_put(self._capture_queue, self.__img.copy())
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"Capture error: {e}")
|
||||
|
||||
def _flow_worker(self):
|
||||
"""Stage 2: Compute optical flow by reusing __compute_flow()."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
img = self._capture_queue.get(timeout=0.1)
|
||||
# Set __img to reuse __compute_flow()
|
||||
self.__img = img
|
||||
self.__compute_flow()
|
||||
flow = self.__flow.copy() if self.__flow is not None else None
|
||||
self._queue_put(self._flow_queue, (img, flow))
|
||||
except Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"Flow error: {e}")
|
||||
|
||||
def _hhd_worker(self):
|
||||
"""Stage 3: HHD decomposition by reusing __compute_hhd()."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
img, flow = self._flow_queue.get(timeout=0.1)
|
||||
|
||||
# Set __flow to reuse __compute_hhd()
|
||||
self.__flow = flow
|
||||
self.__compute_hhd()
|
||||
# Get results from internal variables set by __compute_hhd()
|
||||
result = {
|
||||
'img': img, 'flow': flow,
|
||||
'normal': self.__vnormal.copy() if self.__vnormal is not None else None,
|
||||
'shear': self.__vshear.copy() if self.__vshear is not None else None,
|
||||
# 'depth': getFilteredDensity(flow, use_cuda=self.cuda),
|
||||
'fnormal': self.__fnormal, 'fshearx': self.__fshearx,
|
||||
'fsheary': self.__fsheary, 'ftor': self.__ftor,
|
||||
}
|
||||
|
||||
# Always include contact fields to ensure consistent result structure
|
||||
# When compute_contact is True, contact worker will update these values
|
||||
# When compute_contact is False, they remain None
|
||||
result.update({
|
||||
'centroid': None,
|
||||
'centroid_found': False,
|
||||
'contour': None,
|
||||
'depth_map': None,
|
||||
'depth': None, # Alias for depth_map for compatibility
|
||||
})
|
||||
|
||||
self._queue_put(self._result_queue, result)
|
||||
except Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"HHD error: {e}")
|
||||
|
||||
def _contact_worker(self):
|
||||
"""Stage 4: Contact computation using vnormal from HHD results."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
# Get results from HHD worker which includes vnormal
|
||||
hhd_result = self._result_queue.get(timeout=0.1)
|
||||
vnormal = hhd_result.get('normal') # This is vnormal from HHD
|
||||
|
||||
if vnormal is not None:
|
||||
# Compute contact using vnormal directly
|
||||
contact_result = self.compute_contact(vnormal=vnormal)
|
||||
# Add contact results to the existing HHD result
|
||||
depth_map = contact_result.get('divergence_map')
|
||||
hhd_result.update({
|
||||
'centroid': contact_result.get('centroid'),
|
||||
'centroid_found': contact_result.get('centroid_found'),
|
||||
'contour': contact_result.get('contour'),
|
||||
'depth_map': depth_map,
|
||||
'depth': depth_map, # Alias for depth_map for compatibility
|
||||
})
|
||||
|
||||
# Put updated result back to queue
|
||||
self._queue_put(self._result_queue, hhd_result)
|
||||
except Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"Contact error: {e}")
|
||||
|
||||
# =========================================================================
|
||||
# Private - Utilities
|
||||
@@ -705,3 +500,194 @@ class Sensor:
|
||||
print(f" {k}: {v:7.2f} ms")
|
||||
print(f" ─────────────────────────")
|
||||
print(f" TOTAL: {timings['total']:7.2f} ms ({1000/timings['total']:.1f} FPS)")
|
||||
|
||||
|
||||
|
||||
|
||||
# =========================================================================
|
||||
# Public API - Async Pipeline
|
||||
# =========================================================================
|
||||
|
||||
# def enable_async(self, queue_size=2, compute_contact=False):
|
||||
# """
|
||||
# Enable 3-stage async pipeline: Capture → Flow → HHD.
|
||||
|
||||
# Each stage runs in parallel for maximum throughput.
|
||||
# """
|
||||
# if self._async_enabled:
|
||||
# return
|
||||
# self._compute_contact_enabled = compute_contact
|
||||
|
||||
# self._capture_queue = Queue(maxsize=queue_size)
|
||||
# self._flow_queue = Queue(maxsize=queue_size)
|
||||
# self._result_queue = Queue(maxsize=queue_size)
|
||||
# if compute_contact:
|
||||
# self._contact_queue = Queue(maxsize=queue_size)
|
||||
# self._stop_event = Event()
|
||||
# self._async_enabled = True
|
||||
|
||||
# self._capture_thread = Thread(target=self._capture_worker, daemon=True)
|
||||
# self._flow_thread = Thread(target=self._flow_worker, daemon=True)
|
||||
# self._compute_thread = Thread(target=self._hhd_worker, daemon=True)
|
||||
# if compute_contact:
|
||||
# self._contact_thread = Thread(target=self._contact_worker, daemon=True)
|
||||
|
||||
# self._capture_thread.start()
|
||||
# self._flow_thread.start()
|
||||
# self._compute_thread.start()
|
||||
# if compute_contact:
|
||||
# self._contact_thread.start()
|
||||
|
||||
# if self.verbose:
|
||||
# if compute_contact:
|
||||
# print("Async pipeline enabled: Capture → Flow → HHD → Contact")
|
||||
# else:
|
||||
# print("Async pipeline enabled: Capture → Flow → HHD")
|
||||
|
||||
# def get_async(self, timeout=0.05):
|
||||
# """
|
||||
# Get latest result from async pipeline (non-blocking).
|
||||
|
||||
# Returns:
|
||||
# dict or None: Sensor data if available
|
||||
# """
|
||||
# compute_contact = self._compute_contact_enabled
|
||||
# if not self._async_enabled:
|
||||
# return None
|
||||
# try:
|
||||
# result = self._result_queue.get(timeout=timeout)
|
||||
# t = time.perf_counter()
|
||||
# result['fps'] = 1.0 / (t - self.last_log_time) if self.last_log_time else 0
|
||||
# self.last_log_time = t
|
||||
|
||||
# # Sync instance variables
|
||||
# # for k in ['img', 'flow', 'normal', 'shear', 'depth', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
# if compute_contact:
|
||||
# for k in ['img', 'flow', 'normal', 'shear', 'depth', 'centroid', 'centroid_found', 'contour', 'fps', 'fnormal', 'fshearx', 'fsheary', 'ftor']:
|
||||
# setattr(self, k, result.get(k))
|
||||
# else:
|
||||
# for k in ['img', 'flow', 'normal', 'shear', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
# setattr(self, k, result.get(k))
|
||||
# self.vupdate = True
|
||||
# return result
|
||||
# except Empty:
|
||||
# return None
|
||||
|
||||
# def disable_async(self):
|
||||
# """Stop async pipeline."""
|
||||
# if not self._async_enabled:
|
||||
# return
|
||||
# self._stop_event.set()
|
||||
# for t in [self._capture_thread, self._flow_thread, self._compute_thread]:
|
||||
# if t: t.join(timeout=2.0)
|
||||
# self._async_enabled = False
|
||||
# self._capture_thread = self._flow_thread = self._compute_thread = None
|
||||
# self._capture_queue = self._flow_queue = self._result_queue = self._stop_event = None
|
||||
# if self.verbose:
|
||||
# print("Async pipeline disabled")
|
||||
|
||||
# =========================================================================
|
||||
# Private - Async Workers
|
||||
# =========================================================================
|
||||
|
||||
# def _queue_put(self, q, item):
|
||||
# """Put item in queue, dropping old if full."""
|
||||
# if q.full():
|
||||
# try: q.get_nowait()
|
||||
# except Empty: pass
|
||||
# q.put(item)
|
||||
|
||||
# def _capture_worker(self):
|
||||
# """Stage 1: Capture and preprocess frames by reusing get_img()."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# # Reuse get_img() to avoid code duplication
|
||||
# if self.get_img() is None:
|
||||
# continue
|
||||
|
||||
# # Put the processed image in the queue
|
||||
# # get_img() updates self.__img, so we use that
|
||||
# if self.__img is not None:
|
||||
# self._queue_put(self._capture_queue, self.__img.copy())
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"Capture error: {e}")
|
||||
|
||||
# def _flow_worker(self):
|
||||
# """Stage 2: Compute optical flow by reusing __compute_flow()."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# img = self._capture_queue.get(timeout=0.1)
|
||||
# # Set __img to reuse __compute_flow()
|
||||
# self.__img = img
|
||||
# self.__compute_flow()
|
||||
# flow = self.__flow.copy() if self.__flow is not None else None
|
||||
# self._queue_put(self._flow_queue, (img, flow))
|
||||
# except Empty:
|
||||
# continue
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"Flow error: {e}")
|
||||
|
||||
# def _hhd_worker(self):
|
||||
# """Stage 3: HHD decomposition by reusing __compute_hhd()."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# img, flow = self._flow_queue.get(timeout=0.1)
|
||||
|
||||
# # Set __flow to reuse __compute_hhd()
|
||||
# self.__flow = flow
|
||||
# self.__compute_hhd()
|
||||
# # Get results from internal variables set by __compute_hhd()
|
||||
# result = {
|
||||
# 'img': img, 'flow': flow,
|
||||
# 'normal': self.__vnormal.copy() if self.__vnormal is not None else None,
|
||||
# 'shear': self.__vshear.copy() if self.__vshear is not None else None,
|
||||
# # 'depth': getFilteredDensity(flow, use_cuda=self.cuda),
|
||||
# 'fnormal': self.__fnormal, 'fshearx': self.__fshearx,
|
||||
# 'fsheary': self.__fsheary, 'ftor': self.__ftor,
|
||||
# }
|
||||
|
||||
# # Always include contact fields to ensure consistent result structure
|
||||
# # When compute_contact is True, contact worker will update these values
|
||||
# # When compute_contact is False, they remain None
|
||||
# result.update({
|
||||
# 'centroid': None,
|
||||
# 'centroid_found': False,
|
||||
# 'contour': None,
|
||||
# 'depth_map': None,
|
||||
# 'depth': None, # Alias for depth_map for compatibility
|
||||
# })
|
||||
|
||||
# self._queue_put(self._result_queue, result)
|
||||
# except Empty:
|
||||
# continue
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"HHD error: {e}")
|
||||
|
||||
# def _contact_worker(self):
|
||||
# """Stage 4: Contact computation using vnormal from HHD results."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# # Get results from HHD worker which includes vnormal
|
||||
# hhd_result = self._result_queue.get(timeout=0.1)
|
||||
# vnormal = hhd_result.get('normal') # This is vnormal from HHD
|
||||
|
||||
# if vnormal is not None:
|
||||
# # Compute contact using vnormal directly
|
||||
# contact_result = self.compute_contact(vnormal=vnormal)
|
||||
# # Add contact results to the existing HHD result
|
||||
# depth_map = contact_result.get('divergence_map')
|
||||
# hhd_result.update({
|
||||
# 'centroid': contact_result.get('centroid'),
|
||||
# 'centroid_found': contact_result.get('centroid_found'),
|
||||
# 'contour': contact_result.get('contour'),
|
||||
# 'depth_map': depth_map,
|
||||
# 'depth': depth_map, # Alias for depth_map for compatibility
|
||||
# })
|
||||
|
||||
# # Put updated result back to queue
|
||||
# self._queue_put(self._result_queue, hhd_result)
|
||||
# except Empty:
|
||||
# continue
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"Contact error: {e}")
|
||||
|
||||
|
||||
Reference in New Issue
Block a user