双目测距代码交接

SGBMBinocularVision.py

@@ -0,0 +1,149 @@
+import cv2
+import numpy as np
+import time
+import math
+
+# -----------------------------------双目相机的基本参数---------------------------------------------------------
+#   left_camera_matrix          左相机的内参矩阵
+#   right_camera_matrix         右相机的内参矩阵
+#
+#   left_distortion             左相机的畸变系数    格式(K1,K2,P1,P2,0)
+#   right_distortion            右相机的畸变系数
+# -------------------------------------------------------------------------------------------------------------
+
+# 左镜头的内参，如焦距
+left_camera_matrix = np.array([[716.44292633, 0., 641.67188624], [0., 712.73240811, 321.80519519], [0., 0., 1.]])
+right_camera_matrix = np.array([[714.26431969, 0., 642.69117299], [0., 710.9244224, 329.27392689], [0., 0., 1.]])
+
+# 畸变系数,K1、K2、K3为径向畸变,P1、P2为切向畸变
+left_distortion = np.array([[-0.05928891, 0.12398536, -0.00180906, 0.00385613, -0.51151642]])
+right_distortion = np.array([[-0.0371133, -0.01211102, 0.00179116, 0.00457662, -0.13804965]])
+
+# 旋转矩阵
+R = np.array([[0.99907365, 0.00181788, -0.04299471],
+              [-0.00210211, 0.99997623, -0.00656642],
+              [0.04298175, 0.00665072, 0.99905372]])
+# 平移矩阵
+T = np.array([-59.57562815, -1.61493649, -1.71697302])
+
+size = (1280, 720)
+
+# 通过之前标定的数据计算相关参数，用于畸变矫正
+R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
+                                                                  right_camera_matrix, right_distortion, size, R,
+                                                                  T)
+
+# 校正查找映射表,将原始图像和校正后的图像上的点一一对应起来
+left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
+right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
+print(Q)
+
+# --------------------------鼠标回调函数---------------------------------------------------------
+#   event               鼠标事件
+#   param               输入参数
+# -----------------------------------------------------------------------------------------------
+def onmouse_pick_points(event, x, y, flags, param):
+    if event == cv2.EVENT_LBUTTONDOWN:
+        threeD = param
+        print('\n像素坐标 x = %d, y = %d' % (x, y))
+        # print("世界坐标是：", threeD[y][x][0], threeD[y][x][1], threeD[y][x][2], "mm")
+        print("世界坐标xyz 是：", threeD[y][x][0] / 1000.0, threeD[y][x][1] / 1000.0, threeD[y][x][2] / 1000.0, "m")
+
+        distance = math.sqrt(threeD[y][x][0] ** 2 + threeD[y][x][1] ** 2 + threeD[y][x][2] ** 2)
+        distance = distance / 1000.0  # mm -> m
+        print("距离是：", distance, "m")
+
+
+# 加载视频文件
+capture = cv2.VideoCapture(1)
+capture.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
+capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+WIN_NAME = 'depth'
+cv2.namedWindow(WIN_NAME, cv2.WINDOW_AUTOSIZE)
+
+# 读取视频
+fps = 0.0
+ret, frame = capture.read()
+while capture.isOpened():
+    # 开始计时
+    t1 = time.time()
+    # 是否读取到了帧，读取到了则为True
+    ret, frame = capture.read()
+    # 切割为左右两张图片
+    frame1 = frame[0:720, 0:1280]
+    frame2 = frame[0:720, 1280:2560]
+    # 将BGR格式转换成灰度图片，用于畸变矫正
+    imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
+    imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
+
+    # 重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
+    # 依据MATLAB测量数据重建无畸变图片,输入图片要求为灰度图
+    img1_rectified = cv2.remap(imgL, left_map1, left_map2, cv2.INTER_LINEAR)  # 这一步进行畸变矫正，得到畸变矫正后的图片
+    img2_rectified = cv2.remap(imgR, right_map1, right_map2, cv2.INTER_LINEAR)
+
+    # 转换为opencv的BGR格式
+    imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR)
+    imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR)
+
+    # ------------------------------------SGBM算法----------------------------------------------------------
+    #   blockSize                   深度图成块，blocksize越低，其深度图就越零碎，0<blockSize<10
+    #   img_channels                BGR图像的颜色通道，img_channels=3，不可更改
+    #   numDisparities              SGBM感知的范围，越大生成的精度越好，速度越慢，需要被16整除，如numDisparities
+    #                               取16、32、48、64等
+    #   mode                        sgbm算法选择模式，以速度由快到慢为：STEREO_SGBM_MODE_SGBM_3WAY、
+    #                               STEREO_SGBM_MODE_HH4、STEREO_SGBM_MODE_SGBM、STEREO_SGBM_MODE_HH。精度反之
+    # ------------------------------------------------------------------------------------------------------
+    blockSize = 8
+    img_channels = 3
+    stereo = cv2.StereoSGBM_create(minDisparity=1,
+                                   numDisparities=64,
+                                   blockSize=blockSize,
+                                   P1=8 * img_channels * blockSize * blockSize,
+                                   P2=32 * img_channels * blockSize * blockSize,
+                                   disp12MaxDiff=-1,
+                                   preFilterCap=1,
+                                   uniquenessRatio=10,
+                                   speckleWindowSize=100,
+                                   speckleRange=100,
+                                   mode=cv2.STEREO_SGBM_MODE_HH)
+    # 计算视差
+    disparity = stereo.compute(img1_rectified, img2_rectified)
+
+    # 归一化函数算法，生成深度图（灰度图）
+    disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
+
+    # 生成深度图（颜色图）
+    dis_color = disparity
+    dis_color = cv2.normalize(dis_color, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
+    dis_color = cv2.applyColorMap(dis_color, 2)
+
+    # 计算三维坐标数据值
+    threeD = cv2.reprojectImageTo3D(disparity, Q, handleMissingValues=True)
+    # 计算出的threeD，需要乘以16，才等于现实中的距离
+    threeD = threeD * 16
+
+    # 鼠标回调事件
+    cv2.setMouseCallback("depth", onmouse_pick_points, threeD)
+
+    # 完成计时，计算帧率
+    fps = (fps + (1. / (time.time() - t1))) / 2
+    frame = cv2.putText(frame, "fps= %.2f" % (fps), (0, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+
+    cv2.imshow("depth", dis_color)
+    cv2.imshow("left", frame1)
+    cv2.imshow('Deep disp', disp)  # 显示深度图的双目画面
+    # 若键盘按下q则退出播放
+    if cv2.waitKey(1) & 0xff == ord('q'):
+        break
+
+# 释放资源
+capture.release()
+
+# 关闭所有窗口
+cv2.destroyAllWindows()
+
+
+
+
+

calibration.py

@@ -0,0 +1,65 @@
+import cv2
+import os
+import numpy as np
+
+leftpath = r'.\cap\left\cal'
+rightpath = r'.\cap\right\cal'
+CHECKERBOARD = (4, 4)  # 棋盘格内角点数
+square_size = (30, 30)   # 棋盘格大小，单位mm
+
+criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)  # termination criteria 结束条件
+imgpoints_l = []    # 存放左图像坐标系下角点位置
+imgpoints_r = []    # 存放右图像坐标系下角点位置
+objpoints = []   # 存放世界坐标系下角点位置
+objp = np.zeros((1, CHECKERBOARD[0]*CHECKERBOARD[1], 3), np.float32)
+objp[0, :, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
+objp[0, :, 0] *= square_size[0]
+objp[0, :, 1] *= square_size[1]
+
+
+for ii in range(len(os.listdir(leftpath))):
+    img_l = cv2.imread(os.path.join(leftpath, os.listdir(leftpath)[ii]))
+    gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
+    img_r = cv2.imread(os.path.join(rightpath, os.listdir(rightpath)[ii]))
+    gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
+    ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHECKERBOARD)  # 检测棋盘格内角点。cv2.findChessboardCorners()函数是OpenCV中的一个函数，用于找到棋盘格的所有内角点，返回所有检测到的内角点的2D坐标。
+    ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHECKERBOARD)
+    if ret_l and ret_r:
+        print("计算第"+str(ii)+"对图像")
+        objpoints.append(objp)
+        corners2_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
+        imgpoints_l.append(corners2_l)
+        corners2_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)
+        imgpoints_r.append(corners2_r)
+        img = cv2.drawChessboardCorners(img_l, CHECKERBOARD, corners2_l, ret_l)
+        cv2.imwrite('./ChessboardCorners_img_l.jpg', img)
+
+ret, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(objpoints, imgpoints_l, gray_l.shape[::-1], None, None)  # 先分别做单目标定，利用单目校正函数实现相机内参初始化
+ret, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(objpoints, imgpoints_r, gray_r.shape[::-1], None, None)
+retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(objpoints, imgpoints_l, imgpoints_r, mtx_l, dist_l, mtx_r, dist_r, gray_l.shape[::-1])  # 再做双目标定
+
+print("stereoCalibrate : \n")
+print("Camera matrix left : ")  # Camera matrix 相机内参矩阵
+print(cameraMatrix1)
+print("distCoeffs left  : ")  # distCoeffs 畸变矩阵
+print(distCoeffs1)
+print("cameraMatrix right : ")
+print(cameraMatrix2)
+print("distCoeffs right : ")
+print(distCoeffs2)
+print("R : ")  # 第一和第二个摄像机之间的旋转矩阵
+print(R)
+print("T : ")  # 第一和第二个摄像机之间的平移矩阵
+print(T)
+print("E : ")  # 本质矩阵
+print(E)
+print("F : ")  # 基础矩阵
+print(F)
+np.save('Camera matrix left.npy', cameraMatrix1)
+np.save('distCoeffs left.npy', distCoeffs1)
+np.save('cameraMatrix right.npy', cameraMatrix2)
+np.save('distCoeffs right.npy', distCoeffs2)
+np.save('R.npy', R)
+np.save('T.npy', T)
+np.save('F.npy', F)
+np.save('E.npy', E)

stereoconfig.py

@@ -0,0 +1,38 @@
+import numpy as np
+
+
+# 双目相机参数
+class stereoCamera(object):
+    def __init__(self):
+        # 左相机内参
+        self.cam_matrix_left = np.load("Camera matrix left.npy")
+        # 右相机内参
+        self.cam_matrix_right = np.load('CameraMatrix right.npy')
+
+        # 左右相机畸变系数:[k1, k2, p1, p2, k3]
+        self.distortion_l = np.load('distCoeffs left.npy')
+        self.distortion_r = np.load('distCoeffs right.npy')
+
+        # 旋转矩阵
+        self.R = np.load('R.npy')
+
+        # 平移矩阵
+        self.T = np.load('T.npy')
+
+        # 主点列坐标的差
+        self.doffs = 0.0
+
+        # 指示上述内外参是否为经过立体校正后的结果
+        self.isRectified = False
+
+    def setMiddleBurryParams(self):
+        self.cam_matrix_left = np.load("Camera matrix left.npy")
+        self.cam_matrix_right = np.load('CameraMatrix right.npy')
+
+        self.distortion_l = np.load('distCoeffs left.npy')  # 以下6行代码是添加的
+        self.distortion_r = np.load('distCoeffs right.npy')
+        self.R = np.load('R.npy')
+        self.T = np.load('T.npy')
+        self.doffs = 21.83546358
+        self.isRectified = True
+

takePhotos.py

@@ -0,0 +1,44 @@
+import cv2
+import time
+# AUTO = True  # 自动拍照，或手动按s键拍照
+AUTO = False
+INTERVAL = 2  # 自动拍照间隔
+camera = cv2.VideoCapture(1)  # 设置分辨率左右摄像机同一频率，同一设备ID；左右摄像机总分辨率2560x720；分割为两个1280x720
+camera.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
+camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+counter = 0
+utc = time.time()
+folder = "./takePhoto/"  # 拍照文件目录
+
+
+def shot(pos, frame):
+    global counter
+    path = folder + pos + "_" + str(counter) + ".jpg"
+    cv2.imwrite(path, frame)
+    print("snapshot saved into: " + path)
+
+
+while camera.isOpened():
+    ret, frame = camera.read()
+    left_frame = frame[0:720, 0:1280]
+    right_frame = frame[0:720, 1280:2560]
+    cv2.imshow("left", left_frame)
+    cv2.imshow("right", right_frame)
+
+    now = time.time()
+    if AUTO and now - utc >= INTERVAL:
+        shot("left", left_frame)
+        shot("right", right_frame)
+        counter += 1
+        utc = now
+    key = cv2.waitKey(1)
+    if key == ord("q"):
+        break
+    elif key == ord("s"):
+        shot("left", left_frame)
+        shot("right", right_frame)
+        counter += 1
+camera.release()
+cv2.destroyWindow("left")
+cv2.destroyWindow("right")
+

undistortion.py

@@ -0,0 +1,21 @@
+import cv2
+import glob
+import numpy as np
+a = [0]
+a.append(np.load("F.npy"))
+# a = np.expand_dims(np.load("Camera matrix left.npy"),axis=1)
+
+# b = np.expand_dims(np.loadtxt("Camera matrix left.npy"),axis=0)
+a.append(np.load("Camera matrix left.npy"))
+
+a.append(np.load('Camera matrix right.npy'))
+a.append(np.load('distCoeffs left.npy'))
+
+a.append(np.load('distCoeffs right.npy'))
+a.append(np.load('R.npy'))
+a.append(np.load('T.npy'))
+a.append(np.load('F.npy'))
+a.append(np.load('E.npy'))
+for i in a:
+    print(i)
+    print('\n')