From 04a47cc9397a1c86a268b06d9def132dc5ca9dd0 Mon Sep 17 00:00:00 2001 From: TPP-G6 Date: Wed, 25 Mar 2026 21:32:09 +0800 Subject: [PATCH] update test_stitch.py --- test_ffmpeg.py | 99 ++++++++++----- test_stitch.py | 326 +++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 393 insertions(+), 32 deletions(-) create mode 100644 test_stitch.py diff --git a/test_ffmpeg.py b/test_ffmpeg.py index 8656e46..2ed320f 100644 --- a/test_ffmpeg.py +++ b/test_ffmpeg.py @@ -1,46 +1,81 @@ +import argparse import av import cv2 import time DEVICE = "/dev/video4" TARGET_FPS = 30 -WIDTH = 1280 -HEIGHT = 720 +WIDTH = 1920 +HEIGHT = 1080 -# 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() +def main(): + parser = argparse.ArgumentParser(description="Capture from V4L2 via PyAV, optionally save to MP4.") + parser.add_argument( + "-o", + "--output", + type=str, + default="", + help="Output MP4 path (e.g. capture.mp4). If empty, only preview, no file.", + ) + args = parser.parse_args() -for frame in container.decode(stream): - t0 = time.perf_counter() - img = frame.to_ndarray(format="bgr24") # OpenCV-compatible - t1 = time.perf_counter() + # 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), + }, + ) - cv2.imshow("pyav_cam", img) - key = cv2.waitKey(1) & 0xFF - t2 = time.perf_counter() + stream = container.streams.video[0] + frame_count = 0 + t_start = 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}" + writer = None + if args.output: + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + writer = cv2.VideoWriter( + args.output, fourcc, float(TARGET_FPS), (WIDTH, HEIGHT) ) + if not writer.isOpened(): + container.close() + raise RuntimeError(f"Could not open VideoWriter for {args.output}") + print(f"Recording to {args.output} @ {TARGET_FPS} fps, {WIDTH}x{HEIGHT}") - if key == ord("q"): - break + try: + for frame in container.decode(stream): + t0 = time.perf_counter() + img = frame.to_ndarray(format="bgr24") # OpenCV-compatible + t1 = time.perf_counter() -container.close() -cv2.destroyAllWindows() \ No newline at end of file + if writer is not None: + writer.write(img) + + 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 + finally: + container.close() + if writer is not None: + writer.release() + print(f"Saved {frame_count} frames to {args.output}") + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() diff --git a/test_stitch.py b/test_stitch.py new file mode 100644 index 0000000..f231303 --- /dev/null +++ b/test_stitch.py @@ -0,0 +1,326 @@ +""" +示例 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()