环视运行

saveimg.py

@@ -1,7 +1,3 @@
-# 相机参考工具
-# @路怀帅 2025.12.19 10:32am 
-# PS : 新增 加载相机畸变KD 视频对比显示功能
-
 import cv2
 import threading
 import sys
@@ -16,40 +12,59 @@ running = True
 which_camera = 0
 W, H = 1920, 1080   # 相机分辨率
 
-# 加载鱼眼相机参数
-
-# Front 参数
-# K = np.array([[ 5.3382445956203196e+02, 0., 9.7253025945442369e+02],
-#             [0., 5.3393792084343488e+02, 5.6249605531924215e+02],
-#             [ 0., 0., 1. ]])
-
-# D = np.array([ -1.5749135021037808e-02, 2.9390620422222835e-03,
-#        -4.3176357910129585e-03, 1.3296605027646462e-03 ])
-
-# Right 参数
-K = np.array([[5.3402108990030604e+02, 0., 9.2598444295282172e+02], 
-              [0., 5.3455325152709827e+02, 5.7771767919091610e+02],
-              [0., 0., 1.]])
-D = np.array([-1.8724887233075402e-02, 6.4408558584901701e-03,
-              -5.2069636709412993e-03, 8.4815411645490968e-04])
-# 加载透视变换参数
-
-# Right 参数
-front_proj_matrix = np.array([
-    [-4.5788125154634862e-01, -2.4847281172116515e+00, 1.1901356339453334e+03],
-    [7.0190114936088524e-02, -2.2880693827869965e+00, 1.0012772462548877e+03],
-    [1.4549566908718078e-04, -3.4392525895003334e-03, 1.0]
-])
-
-# 这些参数在 surround_view 中用于 set_scale_and_shift
-scale_xy = np.array([6.99999988e-01, 8.00000012e-01])  # x, y 缩放比例
-shift_xy = np.array([-80., -200.])  # x, y 偏移量 (像素)
-
+def load_camera_params(camera_name):
+    """
+    从YAML文件加载相机参数
+    """
+    yaml_file = os.path.join("yaml", f"{camera_name}.yaml")
+    
+    if not os.path.exists(yaml_file):
+        raise FileNotFoundError(f"YAML file not found: {yaml_file}")
+    
+    # 使用OpenCV读取YAML文件
+    fs = cv2.FileStorage(yaml_file, cv2.FILE_STORAGE_READ)
+    
+    # 读取相机内参矩阵
+    camera_matrix = fs.getNode("camera_matrix").mat()
+    
+    # 读取畸变系数
+    dist_coeffs = fs.getNode("dist_coeffs").mat()
+    
+    # 读取投影矩阵
+    project_matrix = fs.getNode("project_matrix").mat()
+    
+    # 读取缩放参数
+    scale_xy_node = fs.getNode("scale_xy")
+    if scale_xy_node.empty():
+        scale_xy = np.array([1.0, 1.0])
+    else:
+        scale_xy = scale_xy_node.mat().flatten()
+    
+    # 读取偏移参数
+    shift_xy_node = fs.getNode("shift_xy")
+    if shift_xy_node.empty():
+        shift_xy = np.array([0.0, 0.0])
+    else:
+        shift_xy = shift_xy_node.mat().flatten()
+    
+    fs.release()
+    
+    return camera_matrix, dist_coeffs, project_matrix, scale_xy, shift_xy
 
 def video_thread():
     global frame, running
+    
+    # 动态加载当前摄像头的参数
+    try:
+        K, D, front_proj_matrix, scale_xy, shift_xy = load_camera_params(args.i.lower())
+        print(f"[INFO] Loaded parameters for {args.i} camera")
+    except Exception as e:
+        print(f"[ERROR] Failed to load camera parameters: {e}", file=sys.stderr)
+        running = False
+        return
+    
     cap = cv2.VideoCapture(which_camera, cv2.CAP_ANY)
-    cap.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc(*"YUYV"))
+    cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
     cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
     cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
 
@@ -58,6 +73,7 @@ def video_thread():
         running = False
         return
 
+    # 创建修改后的相机矩阵（包含缩放和平移）
     modified_camera_matrix = K.copy()
     modified_camera_matrix[0, 0] *= scale_xy[0]  # fx *= scale_x
     modified_camera_matrix[1, 1] *= scale_xy[1]  # fy *= scale_y
@@ -70,7 +86,6 @@ def video_thread():
     # 鱼眼相机仅畸变
     map3, map4 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, (W, H), cv2.CV_16SC2)
 
-
     while running:
         ret, f = cap.read()
         if not ret:
@@ -80,7 +95,6 @@ def video_thread():
         undistorted = cv2.remap(f, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) 
         undistorted2 = cv2.remap(f, map3, map4, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) 
         
-       
         proj_image = cv2.warpPerspective(
             undistorted, 
             front_proj_matrix, 
@@ -89,7 +103,6 @@ def video_thread():
             borderValue=(0, 0, 0)
         )
 
-
         frame = f.copy()
         birdseye_small = cv2.resize(f, (W//2, H//2))
         undistorted2_small = cv2.resize(undistorted2, (W//2, H//2))
@@ -98,14 +111,9 @@ def video_thread():
         comparison = np.hstack((birdseye_small, undistorted2_small))
         show_video = np.vstack((comparison, proj_image))
     
-        scale = min(1920 / comparison.shape[1], 1080 / comparison.shape[0])
-        if scale < 1:
-            comparison = cv2.resize(comparison, (int(comparison.shape[1] * scale), int(comparison.shape[0] * scale)))
-        comparison = comparison.astype(np.uint8) 
-
         # 设置视频流全屏显示
         text_info = f"Camera: {args.i.upper()}  |  Press 'q' to quit, 's' to screenshot"
-        cv2.putText(comparison, text_info, (10, 30),
+        cv2.putText(show_video, text_info, (10, 30),
                     cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA)
 
         cv2.namedWindow('Video old vs new', cv2.WND_PROP_FULLSCREEN)
@@ -141,12 +149,13 @@ def input_thread():
             break
 
 if __name__ == "__main__":
-
     # 获取用户参数启动
-    parser = argparse.ArgumentParser(description="Example script with argparse")
+    parser = argparse.ArgumentParser(description="Camera Parameter Loading Tool")
 
     # 获取用户输入的摄像头方位 （front back left right）
-    parser.add_argument("--i", type=str, required=True, help="摄像头方位")
+    parser.add_argument("--i", type=str, required=True, 
+                        choices=["front", "back", "left", "right"],
+                        help="Camera direction (front/back/left/right)")
 
     args = parser.parse_args()
 
@@ -165,6 +174,13 @@ if __name__ == "__main__":
         running = False
         exit(1)
 
+    # 检查YAML目录是否存在
+    yaml_dir = "yaml"
+    if not os.path.exists(yaml_dir):
+        print(f"[ERROR] YAML directory not found: {yaml_dir}", file=sys.stderr)
+        print("Please ensure YAML files are in the 'yaml' directory", file=sys.stderr)
+        exit(1)
+
     # 启动视频线程
     vt = threading.Thread(target=video_thread, daemon=True)
     vt.start()
@@ -172,4 +188,4 @@ if __name__ == "__main__":
     # 主线程监听 SSH 输入
     input_thread()
 
-    print("[INFO] Exiting...")
+    print("[INFO] Exiting...")