From 2109a959f3037b03e327e166359e46653d3e687e Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 19 Jan 2026 10:53:51 +0800 Subject: [PATCH] prepare for calibration data collection --- DIS_scale_test.py | 106 +++++++++++++++ RAFT_test.py | 218 ++++++++++++++++++++++++++++++ calibration_data_collection.py | 141 +++++++++++++++++++ config/prototype_of3.json | 4 +- patch_generation/test_hue_gray.py | 4 +- src/orisys/configs/prototype.json | 4 +- src/orisys/sensor.py | 4 +- 7 files changed, 473 insertions(+), 8 deletions(-) create mode 100644 DIS_scale_test.py create mode 100644 RAFT_test.py create mode 100644 calibration_data_collection.py diff --git a/DIS_scale_test.py b/DIS_scale_test.py new file mode 100644 index 0000000..eba3992 --- /dev/null +++ b/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() diff --git a/RAFT_test.py b/RAFT_test.py new file mode 100644 index 0000000..5473f68 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/calibration_data_collection.py b/calibration_data_collection.py new file mode 100644 index 0000000..716ad1a --- /dev/null +++ b/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() diff --git a/config/prototype_of3.json b/config/prototype_of3.json index cf7c989..12d5082 100644 --- a/config/prototype_of3.json +++ b/config/prototype_of3.json @@ -9,8 +9,8 @@ "resolution_stitch": [400, 400] }, "stitcher": { - "min_area_frac": 0.018, - "max_area_frac": 0.035, + "min_area_frac": 0.01, + "max_area_frac": 0.04, "max_aspect_ratio": 100, "index": [1,2,3,6,7,8,11,12,13], "margin": 10, diff --git a/patch_generation/test_hue_gray.py b/patch_generation/test_hue_gray.py index 6ecedc3..f96defa 100644 --- a/patch_generation/test_hue_gray.py +++ b/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,:] diff --git a/src/orisys/configs/prototype.json b/src/orisys/configs/prototype.json index 0c895a0..f7b841b 100644 --- a/src/orisys/configs/prototype.json +++ b/src/orisys/configs/prototype.json @@ -9,8 +9,8 @@ "resolution_stitch": [400, 400] }, "stitcher": { - "min_area_frac": 0.018, - "max_area_frac": 0.035, + "min_area_frac": 0.01, + "max_area_frac": 0.04, "max_aspect_ratio": 100, "index": [1,2,3,6,7,8,11,12,13], "margin": 10, diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index 5bef4d1..a05fc7d 100644 --- a/src/orisys/sensor.py +++ b/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)