Merge branch 'dev' of yahoo.com.hk@192.168.192.190:D:\repo\orisyssdk_git into dev

five_sensors.py

@@ -0,0 +1,296 @@
+"""
+示例 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, cal_path, result_queue, stop_event, target_fps=None):
+    """
+    传感器工作进程函数
+
+    Args:
+        sensor_id: 传感器编号
+        vid_src: 视频源（文件路径或摄像头索引）
+        config_name: 配置名称
+        cal_path: 标定路径
+        result_queue: 结果队列，用于向主进程发送数据
+        stop_event: 停止事件，用于通知工作进程退出
+        target_fps: 目标帧率；若设置则用定时器限速，使多传感器 FPS 接近一致
+    """
+    try:
+        # 在每个进程中创建独立的传感器实例
+        sensor = orisys.Sensor(
+            vid_src,
+            isstitch=True,
+            cuda=True,
+            verbose=False,  # 减少子进程输出
+            config_name=config_name,
+            cal_path=cal_path
+        )
+
+        print(f"传感器 {sensor_id} 已启动" + (f" (target_fps={target_fps})" if target_fps else ""))
+
+        frame_interval = 1.0 / target_fps if target_fps and target_fps > 0 else 0.0
+
+        while not stop_event.is_set():
+            t_start = time.perf_counter()
+            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 Exception:
+                    # 如果队列已满，跳过这一帧
+                    pass
+
+            except Exception as e:
+                print(f"传感器 {sensor_id} 处理错误: {e}")
+                time.sleep(0.1)
+
+            # 步骤7: 精确限速（time.sleep 在 Windows 上精度仅 ~15.6ms，需留足余量后 spin-wait）
+            if frame_interval > 0:
+                deadline = t_start + frame_interval
+                remaining = deadline - time.perf_counter()
+                if remaining > 0.018:
+                    time.sleep(remaining - 0.018)  # coarse sleep; leave 18ms margin for Windows granularity
+                while time.perf_counter() < deadline:
+                    pass  # spin-wait for the last few ms
+        
+        # 清理资源
+        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. 配置多个传感器
+    # =========================================================================
+    # 目标帧率：设为 None 不限速（可跑满 100+ FPS）；设为正数则 sleep 限速，使 FPS ≤ target_fps
+    target_fps = 30  # 不限速；设为 30 等可 cap 帧率
+
+    num_sensors = 5
+    sensor_configs = [
+        {
+            'sensor_id': 0,
+            'vid_src': './s1_480p.mp4',
+            'config_name': './config/prototype_of3_480p.json',
+            'cal_path': './config/dd01a_0_480p.npy'
+        },
+        {
+            'sensor_id': 1,
+            'vid_src': './s1_480p.mp4',  # 可以改为不同的视频源或摄像头索引
+            'config_name': './config/prototype_of3_480p.json',
+            'cal_path': './config/dd01a_0_480p.npy'
+        },
+                {
+            'sensor_id': 2,
+            'vid_src': './s1_480p.mp4',  # 可以改为不同的视频源或摄像头索引
+            'config_name': './config/prototype_of3_480p.json',
+            'cal_path': './config/dd01a_0_480p.npy'
+        },
+                {
+            'sensor_id': 3,
+            'vid_src': './s1_480p.mp4',  # 可以改为不同的视频源或摄像头索引
+            'config_name': './config/prototype_of3_480p.json',
+            'cal_path': './config/dd01a_0_480p.npy'
+        },
+                {
+            'sensor_id': 4,
+            'vid_src': './s1_480p.mp4',  # 可以改为不同的视频源或摄像头索引
+            'config_name': './config/prototype_of3_480p.json',
+            'cal_path': './config/dd01a_0_480p.npy'
+        },
+    ]
+
+    # =========================================================================
+    # 2. 创建进程间通信对象
+    # =========================================================================
+    # 增大队列大小以缓冲更多帧，避免因主进程处理延迟导致的丢帧
+    result_queues = [Queue(maxsize=30) 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'], config['cal_path'],
+                  result_queues[i], stop_events[i], target_fps)
+        )
+        p.start()
+        processes.append(p)
+    
+    print(f"\n已启动 {len(processes)} 个传感器进程")
+    print("按 'q' 退出程序")
+    
+    # 用于存储每个传感器的最新数据
+    latest_data = [None] * len(sensor_configs)
+    
+    # 用于控制打印频率，减少I/O开销
+    print_counter = 0
+    print_interval = 30  # 每30帧打印一次，进一步减少I/O
+    
+    # 用于控制按键检查频率
+    key_check_counter = 0
+    key_check_interval = 5  # 每5帧检查一次按键
+    
+    try:
+        # =====================================================================
+        # 4. 主循环：从队列获取数据并显示
+        # =====================================================================
+        while True:
+            # 从所有队列中快速获取最新数据（高效清空队列，只保留最新帧）
+            for i, queue in enumerate(result_queues):
+                # 快速清空队列，只保留最后一帧
+                data = None
+                # 一次性获取所有可用数据，只保留最后一个
+                while True:
+                    try:
+                        data = queue.get_nowait()
+                    except:
+                        break
+                
+                if data is not None:
+                    latest_data[i] = data
+            
+            # 显示所有传感器的数据
+            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))
+            
+            # 降低打印频率以减少I/O开销（在主循环级别控制）
+            print_counter += 1
+            if print_counter >= print_interval:
+                for data in latest_data:
+                    if data is not None and 'error' not in data:
+                        sid = data['sensor_id']
+                        print(f"传感器 {sid}: FPS={data['fps']:5.1f}")
+                print_counter = 0
+            
+            # 降低按键检查频率以减少开销
+            key_check_counter += 1
+            if key_check_counter >= key_check_interval:
+                key = cv2.waitKey(1) & 0xFF
+                if key == ord('q'):
+                    print("\n退出程序...")
+                    break
+                key_check_counter = 0
+    
+    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()

src/orisys/sensor.py

@@ -7,9 +7,11 @@ and Helmholtz-Hodge decomposition.
 """
 
 import os
+import platform
 import numpy as np
 import cv2
 import time
+import threading
 from enum import Enum, auto
 from typing import Tuple
 # from threading import Thread, Event
@@ -21,6 +23,10 @@ from .HHD import nHHD
 from . import configs 
 from .get_depth import Contact3D
 from .backend_availability import (check_cupy_available, check_opencl_available)
+try:
+    import av  # Optional: used for Linux camera capture path.
+except ImportError:
+    av = None
 
 class Sensor:
     """
@@ -128,6 +134,18 @@ class Sensor:
         self.__contact3d = Contact3D(config_contact=self.__config_contact, verbose=self.verbose)
         self.__hhd = None
         self.poisson_backend = poisson_backend
+        self._use_pyav = False
+        self._av_container = None
+        self._av_stream = None
+        self._av_decoder = None
+        self.cap = None
+        self._is_live_camera = isinstance(self.vid_src, int)
+        self._capture_thread = None
+        self._capture_running = False
+        self._latest_frame = None
+        self._latest_frame_seq = 0
+        self._last_consumed_seq = -1
+        self._latest_frame_lock = threading.Lock()
         
         # Setup video capture
         self._open_video_source(override, box)
@@ -157,7 +175,7 @@ class Sensor:
         # Simple direct read - buffer size of 1 should minimize frame accumulation
         # Timing measurement helps identify if capture or processing is the bottleneck
         t_capture_start = time.perf_counter()
-        ret, frame = self.cap.read()
+        ret, frame = self._read_frame()
         t_capture_end = time.perf_counter()
         self._last_capture_time = t_capture_end - t_capture_start
         
@@ -350,7 +368,20 @@ class Sensor:
 
     def disconnect(self):
         """Release resources and stop async threads."""
-        if self.cap.isOpened():
+        self._capture_running = False
+        if self._capture_thread is not None:
+            self._capture_thread.join(timeout=1.0)
+            self._capture_thread = None
+
+        if self._use_pyav:
+            if self._av_container is not None:
+                self._av_container.close()
+                self._av_container = None
+                self._av_stream = None
+                self._av_decoder = None
+                if self.verbose:
+                    print("Sensor disconnected.")
+        elif self.cap is not None and self.cap.isOpened():
             self.cap.release()
             if self.verbose:
                 print("Sensor disconnected.")
@@ -420,29 +451,57 @@ class Sensor:
 
     def _open_video_source(self, override, box):
         """Initialize video capture and calibration."""
+        use_linux_camera_pyav = (
+            platform.system() == "Linux"
+            and isinstance(self.vid_src, int)
+            and av is not None
+        )
 
-        self.cap = cv2.VideoCapture(self.vid_src)        
+        if use_linux_camera_pyav:
-        # Set buffer size to 1 to reduce latency and avoid frame accumulation
+            device = f"/dev/video{self.vid_src}"
-        # Note: Not all backends support this, but it helps when available
+            try:
-        try:
+                self._av_container = av.open(
-            self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+                    device,
-        except:
+                    format="v4l2",
-            pass  # Some backends don't support this property
+                    options={
-        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
+                        "input_format": "mjpeg",
-        self.cap.set(cv2.CAP_PROP_FPS, self._config_fps)
+                        "video_size": f"{self._config_resolution_raw[0]}x{self._config_resolution_raw[1]}",
-        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0])
+                        "framerate": str(self._config_fps),
-        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1])
+                    },
+                )
+                self._av_stream = self._av_container.streams.video[0]
+                self._av_decoder = self._av_container.decode(self._av_stream)
+                self._use_pyav = True
+                if self.verbose:
+                    print(f"Using PyAV Linux camera backend: {device}")
+            except Exception as exc:
+                if self.verbose:
+                    print(f"PyAV camera open failed ({exc}), fallback to OpenCV backend.")
+                self._use_pyav = False
+
+        if not self._use_pyav:
+            self.cap = cv2.VideoCapture(self.vid_src)
+            # Set buffer size to 1 to reduce latency and avoid frame accumulation
+            # Note: Not all backends support this, but it helps when available
+            try:
+                self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+            except Exception:
+                pass  # Some backends don't support this property
+            self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
+            self.cap.set(cv2.CAP_PROP_FPS, self._config_fps)
+            self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0])
+            self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1])
         
         if self.verbose:
             print("Connecting to video source...")
         
-        if not self.cap.isOpened():
+        if not self._is_source_open():
             raise Exception("Failed to open video source.")
         
         # Wait for first valid frame
         frame = None
         while frame is None:
-            ret, frame = self.cap.read()
+            ret, frame = self._read_frame_raw()
             if not ret:
                 time.sleep(0.1)
 
@@ -463,6 +522,10 @@ class Sensor:
                 self.initial_img = cv2.flip(self.initial_img, 1)
             self.initial_img = self.initial_img.astype(np.uint8)
 
+        # For live cameras, start a latest-frame capture thread to avoid latency buildup.
+        if self._is_live_camera:
+            self._start_capture_thread()
+
 
     def _find_stitch_boxes(self, img_src, show=False):
         """Detect vision tile boxes with camera stabilization."""
@@ -470,7 +533,9 @@ class Sensor:
         start_time = cv2.getTickCount()
         
         while n_boxes < self._config_units:
-            ret, frame = self.cap.read()
+            ret, frame = self._read_frame_raw()
+            if not ret:
+                continue
             v_mean = np.sum(cv2.mean(frame))
             
             if abs(v_mean - v_mean0) < 5:
@@ -496,6 +561,68 @@ class Sensor:
         self.stitcher.extract_tiles()
         return True
 
+    def _is_source_open(self):
+        if self._use_pyav:
+            return self._av_container is not None
+        return self.cap is not None and self.cap.isOpened()
+
+    def _read_frame(self):
+        if self._capture_running:
+            # Startup race guard: wait briefly until capture thread publishes first frame.
+            if self._latest_frame is None:
+                deadline = time.perf_counter() + 1.0
+                while time.perf_counter() < deadline:
+                    with self._latest_frame_lock:
+                        if self._latest_frame is not None:
+                            break
+                    time.sleep(0.001)
+
+            # Return latest frame immediately (do not wait for next camera tick).
+            # This keeps end-to-end latency minimal; caller may receive repeated
+            # frames if processing runs faster than camera FPS.
+            with self._latest_frame_lock:
+                frame = self._latest_frame
+                seq = self._latest_frame_seq
+            if frame is not None:
+                self._last_consumed_seq = seq
+                return True, frame.copy()
+            return False, None
+        return self._read_frame_raw()
+
+    def _read_frame_raw(self):
+        if self._use_pyav:
+            if self._av_decoder is None:
+                return False, None
+            try:
+                frame = next(self._av_decoder)
+            except StopIteration:
+                return False, None
+            except Exception:
+                return False, None
+            return True, frame.to_ndarray(format="bgr24")
+        if self.cap is None:
+            return False, None
+        return self.cap.read()
+
+    def _start_capture_thread(self):
+        if self._capture_thread is not None:
+            return
+        self._capture_running = True
+        self._capture_thread = threading.Thread(target=self._capture_loop, daemon=True)
+        self._capture_thread.start()
+        if self.verbose:
+            print("Latest-frame capture thread started.")
+
+    def _capture_loop(self):
+        while self._capture_running:
+            ret, frame = self._read_frame_raw()
+            if not ret:
+                time.sleep(0.001)
+                continue
+            with self._latest_frame_lock:
+                self._latest_frame = frame
+                self._latest_frame_seq += 1
+
 
     # =========================================================================
     # Private - Utilities

test_cam.py

@@ -0,0 +1,75 @@
+import cv2
+import time
+
+# Toggle these to test whether exposure is limiting delivered FPS.
+USE_MANUAL_EXPOSURE = False
+MANUAL_EXPOSURE = -6  # Typical V4L2/OpenCV manual range is negative values.
+
+cap = cv2.VideoCapture(4, cv2.CAP_V4L2)
+cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
+cap.set(cv2.CAP_PROP_FPS, 30)
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
+cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+
+# For V4L2 in OpenCV: auto-exposure is usually 3, manual is usually 1.
+if USE_MANUAL_EXPOSURE:
+    ok_auto = cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 1)
+    ok_exp = cap.set(cv2.CAP_PROP_EXPOSURE, MANUAL_EXPOSURE)
+else:
+    ok_auto = cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 3)
+    ok_exp = True
+
+actual_fourcc = int(cap.get(cv2.CAP_PROP_FOURCC))
+print(f"Backend: {cap.getBackendName()}")
+print(
+    f"FOURCC: {chr(actual_fourcc & 0xFF)}"
+    f"{chr((actual_fourcc >> 8) & 0xFF)}"
+    f"{chr((actual_fourcc >> 16) & 0xFF)}"
+    f"{chr((actual_fourcc >> 24) & 0xFF)}"
+)
+print(f"Resolution: {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))}x{int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")
+print(f"FPS: {cap.get(cv2.CAP_PROP_FPS)}")
+print(
+    f"Exposure mode: {'manual' if USE_MANUAL_EXPOSURE else 'auto'} "
+    f"(set_auto_ok={ok_auto}, set_exp_ok={ok_exp})"
+)
+print(f"AutoExposure(readback): {cap.get(cv2.CAP_PROP_AUTO_EXPOSURE)}")
+print(f"Exposure(readback): {cap.get(cv2.CAP_PROP_EXPOSURE)}")
+
+frame_count = 0
+start = time.perf_counter()
+while cap.isOpened():
+    t0 = time.perf_counter()
+    ok = cap.grab()
+    t1 = time.perf_counter()
+    if not ok:
+        break
+
+    ret, frame = cap.retrieve()
+    t2 = time.perf_counter()
+    if not ret:
+        continue
+
+    cv2.imshow("webcam", frame)
+    t3 = time.perf_counter()
+    key = cv2.waitKey(1) & 0xFF
+    t4 = time.perf_counter()
+
+    frame_count += 1
+    if frame_count % 30 == 0:
+        elapsed = t4 - start
+        print(
+            f"Frame {frame_count}: "
+            f"grab={1000*(t1-t0):.1f}ms, "
+            f"retrieve={1000*(t2-t1):.1f}ms, "
+            f"imshow={1000*(t3-t2):.1f}ms, "
+            f"waitKey={1000*(t4-t3):.1f}ms, "
+            f"avg_fps={frame_count/elapsed:.1f}"
+        )
+
+    if key == ord('q'):
+        break
+
+cap.release()
+cv2.destroyAllWindows()

test_ffmpeg.py

@@ -0,0 +1,46 @@
+import av
+import cv2
+import time
+
+DEVICE = "/dev/video4"
+TARGET_FPS = 30
+WIDTH = 1280
+HEIGHT = 720
+
+# Open v4l2 device with same style options as ffmpeg CLI
+container = av.open(
+    DEVICE,
+    format="v4l2",
+    options={
+        "input_format": "mjpeg",
+        "video_size": f"{WIDTH}x{HEIGHT}",
+        "framerate": str(TARGET_FPS),
+    },
+)
+
+stream = container.streams.video[0]
+frame_count = 0
+t_start = time.perf_counter()
+
+for frame in container.decode(stream):
+    t0 = time.perf_counter()
+    img = frame.to_ndarray(format="bgr24")  # OpenCV-compatible
+    t1 = time.perf_counter()
+
+    cv2.imshow("pyav_cam", img)
+    key = cv2.waitKey(1) & 0xFF
+    t2 = time.perf_counter()
+
+    frame_count += 1
+    if frame_count % 30 == 0:
+        elapsed = t2 - t_start
+        print(
+            f"Frame {frame_count}: convert={1000*(t1-t0):.1f}ms, "
+            f"imshow+waitKey={1000*(t2-t1):.1f}ms, avg_fps={frame_count/elapsed:.1f}"
+        )
+
+    if key == ord("q"):
+        break
+
+container.close()
+cv2.destroyAllWindows()