""" 示例 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 argparse def main(): parser = argparse.ArgumentParser() parser.add_argument("--video","-v", type=str, default="0", help="Camera ID / Video file path") parser.add_argument("--config","-c", type=str, default="./config/ddjx01.json", help="Configuration file path") parser.add_argument("--cal","-cal", type=str, default="./config/ddjx01.npy", help="Calibration file path") parser.add_argument("--verbose","-verbose", type=bool, default=True, help="Print detailed information") args = parser.parse_args() # 步骤1：创建传感器对象 (必要步骤)， # config_name为配置文件名（内置配置文件名）或路径（自定义配置文件路径） # cal_path为校准文件路径, 每一个设备需要创建/使用一个校准文件 # 方式1: 从视频文件读取 try: input = int(args.video) is_camera = True sensor = orisys.Sensor(input, config_name=args.config, cal_path= args.cal, verbose=args.verbose) except: is_camera = False sensor = orisys.Sensor(args.video, config_name=args.config, cal_path= args.cal, verbose=args.verbose) print("\n按 'q' 退出程序，按 'r' 重置传感器追踪器") # For stabilizing frame-to-frame stitching (reduce oscillation). # Stores the last good transform (mapping t1 -> t0) and exponentially smooths it. H_prev = None smooth_alpha = 0.2 # 0..1; higher => less smoothing, more jitter min_inliers = 30 # if inliers are too low, keep previous transform max_mean_reproj_err = 8.0 # if reprojection error is too high, keep previous transform frame_idx = 0 print_every = 30 # reduce console spam; set to 1 for per-frame output while True: # 步骤2：信息提取 # 步骤 2.1: 获取并拼接图像 (必要步骤) img = sensor.get_img() if img is None: print("Failed to get image (end of stream or capture error). Exiting.") break boxes = sensor.stitcher.boxes # draw boxes for checking # for box in boxes: # x0, y0, x1, y1 = box # cv2.rectangle(sensor.frame, (x0, y0), (x1, y1), (0, 255, 0), 2) # cv2.imshow("boxes", sensor.frame) # cv2.waitKey(0) tiles = sensor.stitcher.tile_list frame_idx += 1 x_offset = 100 t0, t1 = tiles[0], tiles[1] h0, w0 = t0.shape[:2] h1, w1 = t1.shape[:2] # Two rows: tile0 top-left aligned; tile1 on second row shifted by x_offset (px). # If x_offset < 0, pad canvas on the left so both tiles stay visible. pad_left = max(0, -x_offset) x0 = pad_left x1 = pad_left + x_offset canvas_w = max(x0 + w0, x1 + w1) canvas_h = h0 + h1 if len(t0.shape) == 3: canvas = np.zeros((canvas_h, canvas_w, t0.shape[2]), dtype=t0.dtype) else: canvas = np.zeros((canvas_h, canvas_w), dtype=t0.dtype) canvas[0:h0, x0 : x0 + w0] = t0 canvas[h0 : h0 + h1, x1 : x1 + w1] = t1 # ============================================================ # OpenCV stitcher (BFMatcher + Homography) for t0 / t1 # ============================================================ # Assumption: t0 is the reference image, t1 will be warped onto t0. # This is for visualization / sanity-checking (not the full orisys pipeline). def to_bgr(x): if x.ndim == 2: return cv2.cvtColor(x, cv2.COLOR_GRAY2BGR) return x def to_gray(x): if x.ndim == 2: return x return cv2.cvtColor(x, cv2.COLOR_BGR2GRAY) t0_bgr = to_bgr(t0) t1_bgr = to_bgr(t1) g0 = to_gray(t0_bgr) g1 = to_gray(t1_bgr) # ORB expects 8-bit single-channel images. def ensure_u8_gray(gray): if gray.dtype == np.uint8: return gray gray_u8 = cv2.normalize(gray, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) return gray_u8 g0 = ensure_u8_gray(g0) g1 = ensure_u8_gray(g1) # Debug dtypes/ranges (cheap sanity-check) print( f"BF stitcher debug: t0 dtype={t0.dtype}, t1 dtype={t1.dtype}, " f"g0 dtype={g0.dtype}, g0 range=({np.min(g0)}, {np.max(g0)}), " f"g1 dtype={g1.dtype}, g1 range=({np.min(g1)}, {np.max(g1)})" ) # Mild blur + histogram equalization can improve keypoint detection. g0 = cv2.GaussianBlur(g0, (5, 5), 0) g1 = cv2.GaussianBlur(g1, (5, 5), 0) g0 = cv2.equalizeHist(g0) g1 = cv2.equalizeHist(g1) # ORB + BFMatcher (Hamming) for binary descriptors # ============================ # Tuning guidance (important) # ============================ # If the stitched position looks wrong, tune in this order: # 1) ORB keypoint count/quality: # - Increase/decrease `nfeatures`. # - Adjust `fastThreshold` (lower => more keypoints, potentially more outliers). # - Adjust `edgeThreshold` (lower => more keypoints near edges). # 2) Match filtering (Lowe ratio): # - Lower `ratio` => fewer but cleaner matches (more stable homography). # - Higher `ratio` => more matches, but can increase wrong correspondences. # 3) Geometric fitting: # - `cv2.findHomography(..., cv2.RANSAC, reproj_thresh)`: # * increase reproj_thresh if points are noisy # * decrease reproj_thresh if the model drifts too much # 4) Debug: # - Watch `BFMatcher good matches:` and `Estimated homography inliers:`. # If inliers are low, tighten ratio / reduce reproj_thresh or improve ORB inputs. # Note: this code estimates a homography mapping `t1 -> t0`. orb = cv2.ORB_create(nfeatures=5000, scaleFactor=1.2, nlevels=8, edgeThreshold=15, fastThreshold=5) k0, d0 = orb.detectAndCompute(g0, None) k1, d1 = orb.detectAndCompute(g1, None) if d0 is None or d1 is None or len(k0) < 4 or len(k1) < 4: print( "OpenCV BFMatcher stitcher: not enough features.", f"k0={0 if k0 is None else len(k0)}", f"k1={0 if k1 is None else len(k1)}", f"d0_is_none={d0 is None}", f"d1_is_none={d1 is None}", ) else: bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) matches = bf.knnMatch(d0, d1, k=2) # (query=t0, train=t1) # Ratio test (Lowe) good = [] ratio = 0.9 for m_n in matches: if len(m_n) != 2: continue m, n = m_n if m.distance < ratio * n.distance: good.append(m) print(f"BFMatcher good matches: {len(good)}") # Visualize matches (knn not required for drawMatches; we draw only "good") if len(good) >= 4: pts0 = np.float32([k0[m.queryIdx].pt for m in good]).reshape(-1, 1, 2) pts1 = np.float32([k1[m.trainIdx].pt for m in good]).reshape(-1, 1, 2) # Estimate transform (prefer affine to reduce oscillation) # Mapping model: t1 -> t0 ransac_reproj_thresh = 5.0 H = None inlier_mask = None method = "affine_partial2d" M, inliers_aff = cv2.estimateAffinePartial2D( pts1, pts0, method=cv2.RANSAC, ransacReprojThreshold=ransac_reproj_thresh, ) if M is not None: H = np.vstack([M, [0, 0, 1]]).astype(np.float64) inlier_mask = inliers_aff else: method = "homography" H, inlier_mask = cv2.findHomography(pts1, pts0, cv2.RANSAC, ransac_reproj_thresh) inliers = int(inlier_mask.sum()) if inlier_mask is not None else 0 print(f"Estimated {method} inliers: {inliers}") # Draw matches and inlier points draw = cv2.drawMatches( t0_bgr, k0, t1_bgr, k1, good, None, matchColor=(0, 255, 0), singlePointColor=(255, 0, 0), flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS ) cv2.imshow("BFMatcher Matches (t0 vs t1)", draw) if H is not None: # Normalize homography scale so blending is stable. if abs(H[2, 2]) > 1e-12: H = H / H[2, 2] # Stability check: reprojection error on inliers mean_err = 1e9 if inlier_mask is not None: inlier_bool = inlier_mask.ravel().astype(bool) if np.any(inlier_bool): projected = cv2.perspectiveTransform(pts1[inlier_bool], H) err = np.linalg.norm( projected.reshape(-1, 2) - pts0[inlier_bool].reshape(-1, 2), axis=1 ) mean_err = float(err.mean()) if err.size else 1e9 # Choose which transform to use H_use = H if H_prev is not None: if inliers < min_inliers or mean_err > max_mean_reproj_err: # Too few/too noisy inliers: keep last good transform H_use = H_prev else: # Exponential moving average to damp oscillations H_use = smooth_alpha * H + (1.0 - smooth_alpha) * H_prev if abs(H_use[2, 2]) > 1e-12: H_use = H_use / H_use[2, 2] H_prev = H_use # Warp to a FIXED canvas anchored on t0 (prevents visual "jumping") h0c, w0c = t0_bgr.shape[:2] pad = max(50, int(0.1 * w0c)) out_w = w0c + 2 * pad out_h = h0c + 2 * pad # Shift t0 by pad in the output canvas T_pad = np.array([[1, 0, pad], [0, 1, pad], [0, 0, 1]], dtype=np.float64) H_total = T_pad @ H_use if frame_idx % print_every == 0: # Print transform + where t1 corners land on the output canvas h1c, w1c = t1_bgr.shape[:2] corners1 = np.float32([[0, 0], [w1c, 0], [w1c, h1c], [0, h1c]]).reshape(-1, 1, 2) corners1_warped = cv2.perspectiveTransform(corners1, H_total).reshape(-1, 2) corners0_out = np.float32([ [pad, pad], [pad + w0c, pad], [pad + w0c, pad + h0c], [pad, pad + h0c], ]) # === Stitch transform debug === # H_use (t1 -> t0): # [[ 9.9500020e-01 -3.5779362e-03 8.9181457e+01] # [ 3.5779362e-03 9.9500020e-01 8.5183984e+00] # [ 0.0000000e+00 0.0000000e+00 1.0000000e+00]] # H_total (output canvas): # [[ 9.95000200e-01 -3.57793620e-03 1.39181457e+02] # [ 3.57793620e-03 9.95000200e-01 5.85183984e+01] # [ 0.00000000e+00 0.00000000e+00 1.00000000e+00]] # t1 corners warped into output (x,y): [[139.18146 58.5184 ] # [387.93152 59.412884] # [387.03702 308.16293 ] # [138.28697 307.26846 ]] # t0 corners in output (x,y): [[ 50. 50.] # [300. 50.] # [300. 300.] # [ 50. 300.]] print("=== Stitch transform debug ===") print("H_use (t1 -> t0):\n", H_use) print("H_total (output canvas):\n", H_total) print("t1 corners warped into output (x,y):", corners1_warped) print("t0 corners in output (x,y):", corners0_out) stitched = cv2.warpPerspective( t1_bgr, H_total, (out_w, out_h), flags=cv2.INTER_LINEAR, ) # Paste t0 on top (no blending; good for debugging) stitched[pad:pad + h0c, pad:pad + w0c] = t0_bgr cv2.imshow("OpenCV Stitch (stabilized)", stitched) else: print("OpenCV stitcher: transform is None.") cv2.imshow("tiles_1_2 (two rows, 2nd shifted)", canvas) # cv2.waitKey(0) # 步骤 2.2: 计算形变场 (必要步骤) # is_touched = sensor.compute_deformation(check_motion=True, threshold=6) # 步骤 2.3: 计算触碰点 #步骤 3.5: 键盘控制 key = cv2.waitKey(1) & 0xFF if key == ord("q"): print("\n退出程序...") break if key == ord("r"): # 重置光流追踪器（当追踪漂移时使用） sensor.reset() print("传感器已重置") # ========================================================================= # 4. 清理资源 # ========================================================================= sensor.disconnect() # 释放视频源和相关资源 if __name__ == '__main__': main()