prepare for calibration data collection

DIS_scale_test.py

@@ -0,0 +1,106 @@
+"""
+示例 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
+import time
+def main():
+    
+    # 步骤1：创建传感器对象 (必要步骤)，config_name为配置文件名（内置配置文件名）或路径（自定义配置文件路径）
+    # 方式1: 从视频文件读取
+    sensor1 = orisys.Sensor("s1.mp4", config_name="./config/prototype_of1.json", cal_path= "./config/cal.npy", isstitch=True, cuda=True, verbose=True)
+    sensor2 = orisys.Sensor("s1.mp4", config_name="./config/prototype_of2.json", cal_path= "./config/cal.npy", isstitch=True, cuda=True, verbose=True)
+
+    
+    # 方式2: 从摄像头读取（取消注释使用） 
+    # sensor = orisys.Sensor(0, config_name="./config/lens_test.json", cal_path= "./config/cal.npy", isstitch=True, cuda=True, verbose=True)
+
+    print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")
+
+
+    while True:
+        # 步骤2：信息提取
+        # 步骤 2.1: 获取并拼接图像 (必要步骤)
+        sensor1.get_img() 
+        sensor2.get_img() 
+        # if sensor.detect_motion(threshold=2):
+        #     print("motion detected")
+        # 步骤 2.2: 计算形变场 (必要步骤)
+        sensor1.compute_deformation()
+        sensor2.compute_deformation()
+
+    
+        #步骤 2.4: 读取信息
+        fps, img, flow = sensor1.read_info(
+            sensor1.info.FPS,
+            sensor1.info.IMG,
+            sensor1.info.VRAW,
+        )
+        fps2, img2, flow2 = sensor2.read_info(
+            sensor2.info.FPS,
+            sensor2.info.IMG,
+            sensor2.info.VRAW,
+        )
+
+        # flow_mag = np.linalg.norm(flow, axis=2)
+        # flow_mag2 = np.linalg.norm(flow2, axis=2)
+        # flow_diff = np.linalg.norm(flow - flow2, axis=2)
+
+        # print(f"flow_mag: {flow_mag.mean()}, {flow_mag.min()}, flow_mag2: {flow_mag2.mean()}, {flow_mag2.min()}, flow_diff: {flow_diff.mean()}, {flow_diff.min()}")
+        # flow_diff_norm = cv2.applyColorMap((flow_diff*255).astype(np.uint8), cv2.COLORMAP_JET)
+        # cv2.imshow("flow_diff", flow_diff_norm)
+        
+        #步骤 3: 可视化显示
+        # 步骤 3.1: 绘制形变场
+        arrows = orisys.util.draw_arrows(
+            img,
+            flow,
+            threshold=5,
+            grid_spacing=10,
+            arrow_scale=1.0
+        )
+        arrows2 = orisys.util.draw_arrows(
+            img2,
+            flow2,
+            threshold=5,
+            grid_spacing=10,
+            arrow_scale=1.0
+        )
+        cv2.imshow("形变场 Deformation", arrows)
+        cv2.imshow("形变场 Deformation2", arrows2)
+       
+
+        #步骤 3.4: 打印资料
+        print(f"fps: {fps}, fps2: {fps2}")
+            
+        #步骤 3.5: 键盘控制
+        key = cv2.waitKey(1) & 0xFF
+        if key == ord("q"):
+            print("\n退出程序...")
+            break
+        if key == ord("r"):
+            # 重置光流追踪器（当追踪漂移时使用）
+            sensor1.reset()
+            sensor2.reset()
+            print("传感器已重置")
+        
+    # =========================================================================
+    # 4. 清理资源
+    # =========================================================================
+    sensor1.disconnect()  # 释放视频源和相关资源
+    sensor2.disconnect()  # 释放视频源和相关资源
+
+
+if __name__ == '__main__':
+    main()

RAFT_test.py

@@ -0,0 +1,218 @@
+import os
+os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" 
+import cv2
+import torch
+import torchvision.transforms.functional as F
+import torchvision.transforms as T
+from torchvision.models.optical_flow import raft_large, raft_small
+import numpy as np
+import time
+import orisys
+from torchvision.utils import flow_to_image
+import matplotlib.pyplot as plt
+
+def plot(imgs, **imshow_kwargs):
+    if not isinstance(imgs[0], list):
+        # Make a 2d grid even if there's just 1 row
+        imgs = [imgs]
+
+    num_rows = len(imgs)
+    num_cols = len(imgs[0])
+    _, axs = plt.subplots(nrows=num_rows, ncols=num_cols, squeeze=False)
+    for row_idx, row in enumerate(imgs):
+        for col_idx, img in enumerate(row):
+            ax = axs[row_idx, col_idx]
+            img = F.to_pil_image(img.to("cpu"))
+            ax.imshow(np.asarray(img), **imshow_kwargs)
+            ax.set(xticklabels=[], yticklabels=[], xticks=[], yticks=[])
+
+    plt.tight_layout()
+
+def preprocess(batch, resize=None):
+    """
+    Preprocess image batch for RAFT model.
+    
+    Args:
+        batch: Tensor of shape (N, C, H, W)
+        resize: Optional tuple (height, width) to resize. If None, keeps original size.
+    
+    Returns:
+        Preprocessed tensor
+    """
+    transform_list = [
+        T.ConvertImageDtype(torch.float32),
+        T.Normalize(mean=0.5, std=0.5),  # map [0, 1] into [-1, 1]
+    ]
+    
+    if resize is not None:
+        transform_list.append(T.Resize(size=resize))
+    
+    transforms = T.Compose(transform_list)
+    batch = transforms(batch)
+    return batch
+    
+def setup_simple_raft():
+    """
+    Setup a simpler version using available models
+    """
+    # Check for torchvision RAFT (available in torchvision 0.13+)
+    # try:
+    #     import torchvision
+    #     if hasattr(torchvision.models, 'optical_flow'):
+    #         raft = torchvision.models.optical_flow.raft_small(pretrained=True)
+    #         raft = raft.to('cuda')
+    #         raft.eval()
+    #         print("Loaded RAFT small from torchvision")
+    #         return raft
+    # except:
+    #     pass
+    model = raft_small(pretrained=True, progress=False).to('cuda')
+    model = model.eval()
+    # Alternative: Use LiteFlowNet (lighter alternative)
+    print("RAFT loaded successfully")
+    return model
+
+def compute_dense_optical_flow_simple(prev_gray, curr_gray):
+    """
+    Fallback to OpenCV optical flow if RAFT is not available
+    """
+    # Use OpenCV's Farneback optical flow
+    flow = cv2.calcOpticalFlowFarneback(
+        prev_gray, curr_gray, None,
+        pyr_scale=0.5, levels=3, winsize=15,
+        iterations=3, poly_n=5, poly_sigma=1.2, flags=0
+    )
+    
+    # Convert flow to visualization
+    hsv = np.zeros((curr_gray.shape[0], curr_gray.shape[1], 3), dtype=np.uint8)
+    hsv[..., 1] = 255
+    
+    mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1])
+    hsv[..., 0] = ang * 180 / np.pi / 2
+    hsv[..., 2] = cv2.normalize(mag, None, 0, 255, cv2.NORM_MINMAX)
+    
+    flow_rgb = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
+    return flow_rgb
+
+def main_simple():
+    # Initialize camera
+    sensor = orisys.Sensor('s1.mp4', config_name="prototype", cal_path= "./config/cal.npy", isstitch=True, cuda=True, verbose=True)
+    model = setup_simple_raft()
+    device = "cuda" if torch.cuda.is_available() else "cpu"
+    print(f"Using device: {device}")
+    # cap = cv2.VideoCapture(0)
+    # cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+    # cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+    
+    prev_gray = None
+    fps_buffer = []
+    flow_track = None
+    
+    while True:
+        # ret, frame = cap.read()
+        # if not ret:
+        #     break
+        
+        # # Convert to grayscale
+        # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        sensor.get_img() 
+        frame = sensor.img.copy()
+        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        gray = frame
+        # Compute optical flow
+        if prev_gray is not None:
+            start_time = time.time()
+            
+            # Use simple optical flow
+            # flow_viz = compute_dense_optical_flow_simple(prev_gray, gray)
+            
+            # Convert numpy arrays from (H, W, C) to (C, H, W) format
+            # Normalize to [0, 1] range
+            prev_tensor = torch.from_numpy(prev_gray.transpose(2, 0, 1).astype(np.float32) / 255.0)
+            curr_tensor = torch.from_numpy(gray.transpose(2, 0, 1).astype(np.float32) / 255.0)
+            
+            # Preprocess: add batch dimension, preprocess (keeps batch dimension)
+            # preprocess expects (N, C, H, W), returns (N, C, H, W)
+            # Pass resize=None to keep original size (400x400)
+            prev_tensor = preprocess(prev_tensor.unsqueeze(0), resize=None).to(device)  # (1, C, H, W)
+            curr_tensor = preprocess(curr_tensor.unsqueeze(0), resize=None).to(device)  # (1, C, H, W)
+            print(f"prev_tensor shape: {prev_tensor.shape}")
+            
+            # RAFT expects tensors with batch dimension: (batch_size, C, H, W)
+            with torch.no_grad():
+                flow = model(prev_tensor, curr_tensor)
+                predicted_flows = flow[-1]
+                print(f"Flow shape: {predicted_flows.shape}")
+            print(f"dtype = {predicted_flows.dtype}")
+            print(f"shape = {predicted_flows.shape} = (N, 2, H, W)")
+            print(f"min = {predicted_flows.min()}, max = {predicted_flows.max()}")
+                        
+            # Calculate FPS
+            fps = 1.0 / (time.time() - start_time)
+            fps_buffer.append(fps)
+            if len(fps_buffer) > 30:
+                fps_buffer.pop(0)
+            
+            avg_fps = np.mean(fps_buffer)
+            print(f"avg_fps: {avg_fps}")
+            
+
+
+            # grid = [[img1, flow_img] for (img1, flow_img) in zip(img1_batch, flow_imgs)]
+            # plot(grid)
+            # plt.show()
+            # Convert RAFT flow tensor to numpy array for draw_arrows
+            # RAFT flow shape: (1, 2, H, W) where:
+            #   - Channel 0: x-component (horizontal flow)
+            #   - Channel 1: y-component (vertical flow)
+            # Flow values are already in pixel coordinates (not normalized)
+            
+            # Remove batch dimension: (1, 2, H, W) -> (2, H, W)
+            flow_np = predicted_flows[0].cpu().numpy()  # or .squeeze(0)
+            
+            # Transpose to (H, W, 2) format expected by draw_arrows
+            flow_np = flow_np.transpose(1, 2, 0)  # (2, H, W) -> (H, W, 2)
+            if flow_track is None:
+                flow_track = np.zeros_like(flow_np)
+            else:
+                if abs(np.mean(flow_np)) >0.1:
+                    flow_track = flow_track + flow_np
+
+            flow_np = flow_track
+
+            # Flow is already in pixel coordinates, no denormalization needed
+            # Keep as float32 for draw_arrows (it can handle float values)
+            print(f"Flow numpy shape: {flow_np.shape}, dtype: {flow_np.dtype}")
+            print(f"Flow range: x=[{flow_np[:,:,0].min():.2f}, {flow_np[:,:,0].max():.2f}], "
+                  f"y=[{flow_np[:,:,1].min():.2f}, {flow_np[:,:,1].max():.2f}]")
+
+            arrows = orisys.util.draw_arrows(
+                gray,
+                flow_np,
+                threshold=1,
+                grid_spacing=10,
+                arrow_scale=1.0
+            )
+            
+            # Display results
+            # cv2.putText(frame, f"FPS: {avg_fps:.1f}", (10, 30),
+            #            cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+            
+            # cv2.imshow('Original', frame)
+            # cv2.imshow('Grayscale', gray)
+            cv2.imshow('Optical Flow', arrows)
+        
+        prev_gray = gray.copy()
+        
+        #步骤 3.5: 键盘控制
+        key = cv2.waitKey(1) & 0xFF
+        if key == ord("q"):
+            print("\n退出程序...")
+            break
+        if key == ord("r"):
+            # 重置光流追踪器（当追踪漂移时使用）
+            flow_track = None
+            print("传感器已重置")
+    
+if __name__ == "__main__":
+    main_simple()

calibration_data_collection.py

@@ -0,0 +1,141 @@
+import os
+os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" 
+import cv2
+import orisys
+import numpy as np
+import argparse
+
+def main():
+    parser = argparse.ArgumentParser(description='Calibration Data Collection')
+    parser.add_argument('-i','--vid_source', type=str, default='s1.mp4', help='Video Source')
+    parser.add_argument('-id','--device_id', type=int, default=0, help='Device ID')
+
+    args = parser.parse_args()
+    device_id = args.device_id
+    vid_source = args.vid_source
+    sample_name = f"dd01a_{device_id}" #dd01 (平面款), dd01a (平面款 手搓), dd02 (手指款)
+    test_type = "normal" # shearX, shearY, normal
+    print("\n")
+    print(f"Collecting data for device ID: {device_id} on test type: {test_type}, video source: {vid_source}")
+    print("\n")
+    sensor = orisys.Sensor(vid_source, config_name="./config/prototype_of3.json", cal_path= f"./config/{sample_name}.npy", isstitch=True, cuda=True, verbose=True)
+
+    print(f"\n按 'q' 退出程序，按 'r' 重置光流追踪器，按 's' 开始保存，按 'e' 停止保存，按 'j' 减小力，按 'l' 增大力")
+    force = 0 # kgf
+    force_step = 0.1 # kgf
+    force_min = 0 # kgf
+    force_max = 3 # kgf
+
+    is_save = False
+    frame_count = 0
+    writer = None
+    save_path = None
+
+    while True:
+        sensor.get_img() 
+
+        if is_save and writer is not None and sensor.frame is not None:
+            frame_count += 1
+            writer.write(sensor.frame)
+
+        sensor.compute_deformation()
+        sensor.compute_contact()
+
+        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
+        )
+        
+        arrows = orisys.util.draw_arrows(
+            img,
+            flow,
+            threshold=4,
+            grid_spacing=10,
+            arrow_scale=1.0
+        )
+        cv2.putText(arrows, f"FPS: {fps:.0f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
+        cv2.imshow("Deformation", arrows)
+        
+        image_with_foe = orisys.util.draw_contact(img, contour, centroid)
+        cv2.imshow("Contact Centroid", image_with_foe)
+
+        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：蓝色=低散度，红色=高散度）
+        cv2.imshow("Depth Map", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
+
+        key = cv2.waitKey(1) & 0xFF
+
+        if key == ord("s"):
+            save_name = f"{sample_name}_{test_type}_{force:.1f}kgf"
+            save_path = f"./data/{sample_name}/{test_type}/{save_name}.mp4"
+            # Ensure directory exists
+            os.makedirs(os.path.dirname(save_path), exist_ok=True)
+            # Get frame dimensions from the actual frame
+            if sensor.frame is not None:
+                frame_height, frame_width = sensor.frame.shape[:2]
+                writer = cv2.VideoWriter(save_path, cv2.VideoWriter_fourcc(*'mp4v'), 30, (frame_width, frame_height))
+                if writer.isOpened():
+                    is_save = True
+                    print(f"Start writing to {save_path}")
+                else:
+                    print(f"Failed to open video writer for {save_path}")
+                    writer = None
+            else:
+                print("No frame available to determine video dimensions")
+
+        if key == ord("e"):
+            is_save = False
+            if writer is not None:
+                writer.release()
+                writer = None
+            if save_path is not None:
+                print(f"Stop writing to {save_path}, total frames: {frame_count}")
+            else:
+                print(f"Stop writing, total frames: {frame_count}")
+            frame_count = 0
+
+        if key == ord("i"):
+            print("Up arrow pressed")
+        if key == ord("k"):
+            print("Down arrow pressed")
+        if key == ord("j"):
+            force -= force_step
+            if force < force_min:
+                force = force_min
+            print(f"Force: {force:.1f} kgf")
+        if key == ord("l"):
+            force += force_step
+            if force > force_max:
+                force = force_max
+            print(f"Force: {force:.1f} kgf")
+
+
+        if key == ord("q"):
+            print("\n退出程序...")
+            break
+        if key == ord("r"):
+            # 重置光流追踪器（当追踪漂移时使用）
+            sensor.reset()
+            print("传感器已重置")
+
+        
+    # =========================================================================
+    # 4. 清理资源
+    # =========================================================================
+    if writer is not None:
+        writer.release()
+    sensor.disconnect()  # 释放视频源和相关资源
+
+
+if __name__ == '__main__':
+    main()

config/prototype_of3.json

@@ -9,8 +9,8 @@
     "resolution_stitch": [400, 400]
   },
   "stitcher": {
-    "min_area_frac": 0.018,
+    "min_area_frac": 0.01,
-    "max_area_frac": 0.035,
+    "max_area_frac": 0.04,
     "max_aspect_ratio": 100,
     "index": [1,2,3,6,7,8,11,12,13],
     "margin": 10,

patch_generation/test_hue_gray.py

@@ -179,9 +179,9 @@ def custom_gray_conversion(img, r_weight=0.299, g_weight=0.587, b_weight=0.114,
 # -------------------------- Test the Code --------------------------
 if __name__ == "__main__":
     # Replace with your image path
-    IMAGE_PATH = "./data/new_texture.jpg"
+    IMAGE_PATH = "./raw_1.jpg"
     img = cv2.imread(IMAGE_PATH)
-    dx = -80
+    dx = 50
     dy = 0
     tile = img[410+dy:650+dy,940+dx:1180+dx,:]
     

src/orisys/configs/prototype.json

@@ -9,8 +9,8 @@
     "resolution_stitch": [400, 400]
   },
   "stitcher": {
-    "min_area_frac": 0.018,
+    "min_area_frac": 0.01,
-    "max_area_frac": 0.035,
+    "max_area_frac": 0.04,
     "max_aspect_ratio": 100,
     "index": [1,2,3,6,7,8,11,12,13],
     "margin": 10,

src/orisys/sensor.py

@@ -101,7 +101,7 @@ class Sensor:
         
         # Data Variables (public)
         shape = (self.__config_cam["resolution_stitch"][1], self.__config_cam["resolution_stitch"][0])
-        self.img = None
+        self.frame = self.img = None
         self.img_size_raw = None
         self.img_filtered = None
         self.initial_img = None
@@ -164,7 +164,7 @@ class Sensor:
         if not ret:
             print("Failed to get_img from video source.")
             return None
-        
+        self.frame = frame
         # Convert to grayscale
         gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)