fixed async pipeline

example1_basic_pipeline.py

@@ -1,12 +1,13 @@
 """
 示例 1: 基础用法 - 单传感器同步处理
 
-本示例演示 OrisysSensor 的基本使用流程：
+本示例演示 orisys 的基本使用流程：
 1. 初始化传感器（视频文件或摄像头）
-2. 循环获取图像并计算形变
+2. 循环获取图像并计算形变和触碰点
 3. 读取各类传感器数据
+4. 可视化显示
+
 
-适用场景：单传感器、简单应用、学习入门
 """
 import os
 os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" 
@@ -22,6 +23,7 @@ def main():
     
     # 方式2: 从摄像头读取（取消注释使用） 
     sensor = orisys.Sensor(0, cuda=True, verbose=True, config_name="prototype")
+    sensor.export_config(path="./config/exported_config.json") # 导出配置文件供查看和修改
 
     print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")
 

example2_async_pipeline.py

@@ -31,13 +31,13 @@ os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"  # Windows 兼容
 
 import cv2
 import orisys
-
+import numpy as np
 
 def main():
     # =========================================================================
     # 1. 初始化传感器
     # =========================================================================
-    sensor = orisys.Sensor("./s1.mp4",isstitch=True,cuda=True,verbose=True)
+    sensor = orisys.Sensor(0, cuda=True, verbose=True, config_name="prototype")
     
     # =========================================================================
     # 2. 启用异步流水线
@@ -45,7 +45,7 @@ def main():
     # queue_size: 每个阶段间队列的大小
     #   - 较小的值（1-2）：延迟低，但可能丢帧
     #   - 较大的值（3-5）：吞吐稳定，但延迟增加
-    sensor.enable_async(queue_size=2)
+    sensor.enable_async(queue_size=2, compute_contact=True)
     
     print("\n异步模式已启用")
     print("按 'q' 退出，按 'r' 重置光流追踪器")
@@ -80,6 +80,19 @@ def main():
                 )
                 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

example2_config.py

@@ -1,39 +0,0 @@
-import os
-os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
-import cv2
-import orisys
-
-def main():
-    global start_time
-    
-    # 打开视频, config_name 可以根据传感型号选择
-    sensor1 = orisys.Sensor("./s1.mp4",isstitch=True,cuda = True,verbose = True, config_name="prototype",cal_path="./config/cal.npy")
-    # sensor1 = Sensor(0,isstitch=True,cuda = True,verbose = True, config_name="prototype")
-
-    # 打开摄像头
-    # sensor1 = Sensor(0,isstitch=True,cuda = True,verbose = True)
-    while True:
-        sensor1.get_img() # 提取并拼接复眼图像
-        sensor1.compute_deformation() # 计算形变和分解
-        # 提取信息：
-        # 向量场: VRAW, VNORMAL, VSHEAR: 原始，法向，切向 
-        # 帧率，时间戳: FPS, TIMESTAMP
-        # 变形量总和: FNORMAL , FSHEARX, FSHEARY : 法向，切向（X)，切向（Y)
-        fps, fn, fx, fy, flow = sensor1.read_info(sensor1.info.FPS, sensor1.info.FNORMAL, sensor1.info.FSHEARX, sensor1.info.FSHEARY, sensor1.info.VRAW)
-
-        # 显示向量场
-        arrows = orisys.util.draw_arrows(sensor1.img, flow, threshold=5, grid_spacing=40, arrow_scale=1.0)
-        cv2.imshow("Deformation", arrows)
-
-        key = cv2.waitKey(1)
-        if key & 0xFF == ord("q"):
-            print("\n 停止程序")
-            break
-        if key & 0xFF == ord("r"):
-            sensor1.reset()
-    
-    sensor1.disconnect() # 断开传感器连接
-    
-
-if __name__ == '__main__':
-    main()

src/orisys/sensor.py

@@ -202,7 +202,7 @@ class Sensor:
             
         return True
 
-    def compute_deformation(self, log_timing=False):
+    def compute_deformation(self):
         """
         Compute optical flow and decompose into force components.
         
@@ -211,66 +211,76 @@ class Sensor:
         """
         if self.__img is None:
             raise Exception("Warning: Get image first")
-        if log_timing:
-            timings = {}
-            t_start = time.perf_counter()
         
-        # Optical flow
+        self.__compute_flow()
-        if log_timing: t0 = time.perf_counter()
+        self.__compute_hhd()
+        # FPS calculation
+        self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
+        self.last_log_time = time.perf_counter()
+
+        return True
+
+    def __compute_flow(self):
+        if self.__img is None:
+            raise Exception("Warning: Get image first")
+        
         flow = self.__tracker.track(self.__img)
         if hasattr(flow, 'download'):
             flow = flow.download()
-        if log_timing: timings['optical_flow'] = (time.perf_counter() - t0) * 1000
+        self.__flow = flow
+        return True
+    
+    def __compute_hhd(self):
+
+        flow = self.__flow
+        if flow is None:
+            raise Exception("Warning: Compute flow first")
         
         # Initialize HHD solver
         if self.__hhd is None:
             self.__hhd = nHHD(grid=flow.shape[:2], spacings=(18, 18))
 
         # Force computation
-        if log_timing: t0 = time.perf_counter()
         if self.cuda and self.CUPY_AVAILABLE:
             _, forces = self.__hhd.get_force_gpu_v2(flow)
         else:
             _, forces = self.__hhd.get_force(flow)
         self.__fnormal, self.__fshearx, self.__fsheary, self.__ftor = forces
-        if log_timing: timings['hhd_decompose'] = (time.perf_counter() - t0) * 1000
         
         # Get components
         self.__vnormal = self.__hhd.get_normal()
         self.__vshear = self.__hhd.get_shear()
 
-        # # 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
         self.compute_deformation_valid = True
-        # FPS calculation
-        self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
-        self.last_log_time = time.perf_counter()
-        
-        if log_timing:
-            timings['total'] = (time.perf_counter() - t_start) * 1000
-            self._print_timings("compute_deformation", timings)
 
         return True
 
-    def compute_contact(self):
+    def compute_contact(self, vnormal=None):
+        """
+        Compute contact information from vnormal field.
         
+        Args:
+            vnormal: Optional vnormal field. If None, uses self.__vnormal.
+        
+        Returns:
+            dict: Contact result with centroid, contour, and depth_map
+        """
         multiple_foe = False
         
-        if not self.compute_deformation_valid:
+        # Use provided vnormal or fall back to internal state
-            raise Exception("Warning: Compute deformation first")
+        vnormal_to_use = vnormal if vnormal is not None else self.__vnormal
+        
+        if vnormal_to_use is None:
+            raise Exception("Warning: vnormal is required for contact computation")
 
         if multiple_foe:
-            result = self.__contact3d.compute_multiple_foe(self.__vnormal)
+            result = self.__contact3d.compute_multiple_foe(vnormal_to_use)
         else:
             if self.cuda: 
-                result = self.__contact3d.compute_foe_gpu(self.__vnormal)
+                result = self.__contact3d.compute_foe_gpu(vnormal_to_use)
             else:
-                result = self.__contact3d.compute_foe(self.__vnormal)
+                result = self.__contact3d.compute_foe(vnormal_to_use)
 
         self.__centroid = result['centroid']
         self.__centroid_found = result['centroid_found']
@@ -279,7 +289,7 @@ class Sensor:
         self.__depth_map = result['divergence_map']
 
         self.compute_contact_valid = True
-        return True
+        return result
     
 
     # def get_depth(self, denoise_sigma=15):
@@ -368,7 +378,7 @@ class Sensor:
     # Public API - Async Pipeline
     # =========================================================================
     
-    def enable_async(self, queue_size=2):
+    def enable_async(self, queue_size=2, compute_contact=False):
         """
         Enable 3-stage async pipeline: Capture → Flow → HHD.
         
@@ -376,24 +386,32 @@ class Sensor:
         """
         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._flow_thread = Thread(target=self._flow_worker, daemon=True)
-        self._compute_thread = Thread(target=self.__hhd_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):
@@ -403,6 +421,7 @@ class Sensor:
         Returns:
             dict or None: Sensor data if available
         """
+        compute_contact = self._compute_contact_enabled
         if not self._async_enabled:
             return None
         try:
@@ -413,6 +432,10 @@ class Sensor:
             
             # 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
@@ -564,37 +587,29 @@ class Sensor:
         q.put(item)
 
     def _capture_worker(self):
-        """Stage 1: Capture and preprocess frames."""
+        """Stage 1: Capture and preprocess frames by reusing get_img()."""
-        alpha = 0.7
         while not self._stop_event.is_set():
             try:
-                ret, frame = self.cap.read()
+                # Reuse get_img() to avoid code duplication
-                if not ret:
+                if self.get_img() is None:
                     continue
                 
-                gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+                # Put the processed image in the queue
-                self.__stitcher.get_stitch_unit(gray)
+                # get_img() updates self.__img, so we use that
-                self._extract_tiles(gray)
+                if self.__img is not None:
-                img = np.ascontiguousarray(self.__stitcher.stitch_rt_optimized())
+                    self._queue_put(self._capture_queue, self.__img.copy())
-                img = cv2.resize(img, tuple(self._config_resolution_stitch), interpolation=cv2.INTER_CUBIC)
-                
-                if self.__img_filtered is None:
-                    self.__img_filtered = img.astype(np.float32)
-                else:
-                    self.__img_filtered = alpha * img + (1 - alpha) * self.__img_filtered
-                
-                self._queue_put(self._capture_queue, self.img_filtered.astype(np.uint8))
             except Exception as e:
                 if self.verbose: print(f"Capture error: {e}")
 
     def _flow_worker(self):
-        """Stage 2: Compute optical flow."""
+        """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)
-                flow = self.tracker.track(img)
+                # Set __img to reuse __compute_flow()
-                if hasattr(flow, 'download'):
+                self.__img = img
-                    flow = flow.download()
+                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
@@ -602,38 +617,64 @@ class Sensor:
                 if self.verbose: print(f"Flow error: {e}")
 
     def _hhd_worker(self):
-        """Stage 3: HHD decomposition."""
+        """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)
                 
-                if self.hhd is None:
+                # Set __flow to reuse __compute_hhd()
-                    self.hhd = nHHD(grid=flow.shape[:2], spacings=(18, 18))
+                self.__flow = flow
-                
+                self.__compute_hhd()
-                if self.cuda and self.CUPY_AVAILABLE:
+                # Get results from internal variables set by __compute_hhd()
-                    _, forces = self.hhd.get_force_gpu_v2(flow)
+                result = {
-                else:
-                    _, forces = self.hhd.get_force(flow)
-                
-                self._queue_put(self._result_queue, {
                     'img': img, 'flow': flow,
-                    'normal': self.hhd.get_normal(), 'shear': self.hhd.get_shear(),
+                    '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': forces[0], 'fshearx': forces[1],
+                    'fnormal': self.__fnormal, 'fshearx': self.__fshearx,
-                    'fsheary': forces[2], 'ftor': forces[3],
+                    '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."""
+        """Stage 4: Contact computation using vnormal from HHD results."""
         while not self._stop_event.is_set():
             try:
-                img, flow = self._flow_queue.get(timeout=0.1)
+                # Get results from HHD worker which includes vnormal
-                contact_result = self.compute_contact(flow)
+                hhd_result = self._result_queue.get(timeout=0.1)
-                self._queue_put(self._result_queue, contact_result)
+                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: