update test_stitch.py
This commit is contained in:
TPP-G6
+326
@@ -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()
|
||||
Reference in New Issue
Block a user