单目鸟瞰生成显示

saveimg.py

@@ -14,16 +14,36 @@ import numpy as np
 frame = None
 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])
-W, H = 1920, 1080
+# 加载透视变换参数
 
+# 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 video_thread():
@@ -32,14 +52,24 @@ def video_thread():
     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)
-    # cap.set(cv2.CAP_PROP_FPS, 30)
 
     if not cap.isOpened():
         print("[ERROR] Cannot open camera", file=sys.stderr)
         running = False
         return
-    # 鱼眼相机去畸变
-    map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, (W, H), cv2.CV_16SC2)
+
+    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
+    modified_camera_matrix[0, 2] += shift_xy[0]  # cx += shift_x
+    modified_camera_matrix[1, 2] += shift_xy[1]  # cy += shift_y
+    
+    # 鱼眼相机去畸变 合并缩放系数
+    map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), modified_camera_matrix, (W, H), cv2.CV_16SC2)
+
+    # 鱼眼相机仅畸变
+    map3, map4 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, (W, H), cv2.CV_16SC2)
+
 
     while running:
         ret, f = cap.read()
@@ -47,10 +77,27 @@ def video_thread():
             break
 
         # 图像去畸变
-        undistorted = cv2.remap(f, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
-        # 拼接原视频和抗畸变后的视频 （左右显示）
-        comparison = np.hstack((f, undistorted))
+        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, 
+            (W, H),  # 输出大小 (与原始分辨率一致)
+            borderMode=cv2.BORDER_CONSTANT,
+            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))
+
+        # 拼接原视频和抗畸变后的视频 （左右显示）
+        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)))
@@ -63,7 +110,7 @@ def video_thread():
 
         cv2.namedWindow('Video old vs new', cv2.WND_PROP_FULLSCREEN)
         cv2.setWindowProperty('Video old vs new', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
-        cv2.imshow('Video old vs new', comparison)
+        cv2.imshow('Video old vs new', show_video)
 
         if cv2.waitKey(1) & 0xFF == ord('q'):
             running = False
@@ -74,14 +121,14 @@ def video_thread():
 
 def input_thread():
     global running
-    print("Commands: 's' = screenshot, 'q' = quit")
+    print("SSH命令: 's' = 截图, 'q' = 退出")
     while running:
         try:
             cmd = input().strip().lower()
             if cmd == 's':
                 if frame is not None:
-                    filename = f"./save_img/shot_{datetime.now().strftime('%Y%m%d_%H%M%S')}.jpg"
-                    cv2.imwrite(filename, frame)
+                    filename = f"./images/{args.i.lower()}.png"
+                    cv2.imwrite(filename, frame) 
                     print(f"[SSH] Saved: {os.path.abspath(filename)}")
                 else:
                     print("[SSH] No frame available yet.")