雙目測距python實現(xiàn)方法實戰(zhàn)(含標定)

更新時間：2024年11月05日 10:17:07 作者：simulacra_

相機標定對于雙目視覺系統(tǒng)至關(guān)重要,它涉及到獲取左右相機的內(nèi)參矩陣、畸變系數(shù)、旋轉(zhuǎn)矩陣和平移向量等關(guān)鍵參數(shù),這篇文章主要介紹了雙目測距python實現(xiàn)的相關(guān)資料,需要的朋友可以參考下

雙目標定

參數(shù)介紹

相機標定主要是為了獲取左右相機的內(nèi)參矩陣、畸變系數(shù)、旋轉(zhuǎn)矩陣和平移向量，方便后續(xù)對獲得的圖像進行畸變矯正和圖像對齊。

內(nèi)參矩陣是描述相機內(nèi)在參數(shù)的3x3矩陣，包含了相機的焦距和光學中心的位置。這些參數(shù)定義了相機的成像過程中的投影關(guān)系，內(nèi)參矩陣一般表示為：

其中：

fx 和 fy 是焦距（以像素為單位），分別對應x和y方向。
cx 和 cy 是光學中心（主點），即圖像的中心點。
s稱為視向畸變系數(shù)，它用于校正圖像中的非正交性，通常表示相機的非正交性，即像素在圖像平面上的非直角投影。

畸變系數(shù)用于描述鏡頭的畸變情況，通常包括徑向畸變和切向畸變，通常表示為[k1 k2 p1 p2 k3]，

其中：

k1,k2,k3 是徑向畸變系數(shù)。
p1,p2 是切向畸變系數(shù)。

旋轉(zhuǎn)矩陣用于描述兩個相機之間的旋轉(zhuǎn)關(guān)系，是一個3x3矩陣，表示從右相機坐標系到左相機坐標系的旋轉(zhuǎn)。

平移向量用于描述兩個相機之間的平移關(guān)系，是一個3x1向量，表示從右相機到左相機的平移量。

鏡頭畸變主要分為徑向畸變和切向畸變。徑向畸變使得圖像的直線變成彎曲的曲線，特別是在圖像的邊緣區(qū)域，常見于魚眼鏡頭等廣角鏡頭；切向畸變使得圖像中的點在兩個方向上都發(fā)生位移，通常是由于相機鏡頭和圖像傳感器不完全平行導致的。通過內(nèi)參矩陣和畸變系數(shù)，可以消除圖像中的徑向和切向畸變，使圖像中的直線恢復為直線。

圖像對齊是為了確保雙目相機拍攝的圖像對齊，即每一對圖像中的相同點在兩幅圖像中的垂直位置是相同的，使得每個像素行在兩個圖像中的位置相同，從而方便后續(xù)的立體匹配和深度計算。

標定步驟

標定采用的黑白棋盤格單元格長度最好為2cm，且棋盤格格數(shù)最好為奇數(shù)*偶數(shù)，我采用的標定板為如圖所示的A4紙打印的9*10的黑白棋盤格，標定棋盤格如果太大（如A3）會導致無法保證棋盤格在視野的各個位置（上下左右中）都出現(xiàn)在視野范圍內(nèi)。
標定使用的照片20張左右為宜，不是越多越好。
圖片拍攝中要盡可能在相機視野范圍的各個位置都出現(xiàn)（可以將視野劃分為九宮格，每個格子都要保證有出現(xiàn)過標定板）拍攝的背景盡可能整潔不要有太多干擾物，同時也要有角度的變化（標定板跟相機的夾角最大不超過30度）。
拍攝時固定相機位置旋轉(zhuǎn)標定板，光線對標定效果影響很大，如果標定效果不理想可以多打光，可以使角點更容易識別，角點的梯度邊緣占的像素3-4還可以，1-2算不錯，雙目相機如果沒有自帶的拍照軟件可以拍照后在電腦的畫圖中將圖片放大800%以上，也可以看到角點的像素。下圖為示例，其中一個塊就是一個像素點。

得到標定圖片后，打開Matlab 上方的工具欄APP，找到圖像處理和計算機視覺下的Stereo Camera Calibration工具，打開并將左右相機的圖片導入，一般要求平均像素誤差在0.1左右

如果雙目相機沒有自帶的拍照軟件，可以采用如下python代碼實現(xiàn)拍照和保存照片：

import cv2
imageWidth = 1280
imageHeight = 720

cap = cv2.VideoCapture(1)

# 檢查和設(shè)置攝像頭分辨率
print("Setting camera resolution to:", imageWidth * 2, "x", imageHeight)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, imageWidth * 2)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, imageHeight)

# 讀取設(shè)置后實際的分辨率
actual_width = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
actual_height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
print("Actual resolution set:", actual_width, "x", actual_height)

i = 0

if not cap.isOpened():
    print("Error: Could not open video capture.")
    exit()

while True:
    success, img = cap.read()
    if success:
        print(f"Captured image shape: {img.shape}")

        # 檢查圖像分辨率
        if img.shape[1] >= imageWidth * 2 and img.shape[0] >= imageHeight:
            rgbImageL = img[:, 0:imageWidth, :]
            rgbImageR = img[:, imageWidth:imageWidth * 2, :]
            cv2.imshow('Left', rgbImageL)
            cv2.imshow('Right', rgbImageR)

            c = cv2.waitKey(1) & 0xff
            if c == 13:  # Enter鍵
                cv2.imwrite('Left%d.png' % i, rgbImageL)
                cv2.imwrite('Right%d.png' % i, rgbImageR)
                print("Save %d image" % i)
                i += 1
            elif c == 27:  # ESC鍵
                break
        else:
            print("Captured image width is less than expected.")

cap.release()
cv2.destroyAllWindows()

雙目測距

代碼

獲取標定的參數(shù)后，導入需要的庫，并保存左右相機的內(nèi)參矩陣、畸變系數(shù)等：

import cv2
import numpy as np
import time
import random
import math

# 左鏡頭的內(nèi)參，如焦距
left_camera_matrix = np.array([[676.6671663839634,-1.444673028,596.1585382374480],[0,682.0928218925224,335.1990227822088],[0.,0.,1.]])
right_camera_matrix = np.array([[691.0595574749669,-1.872845542285230,562.1201347823666],[0,693.9359720639367,331.4063679099969],[0.,0.,1.]])

# 畸變系數(shù),K1、K2、K3為徑向畸變,P1、P2為切向畸變
left_distortion = np.array([[0.0979,-0.1404, -2.3303e-04,5.1338e-04,0]])
right_distortion = np.array([[0.1060,-0.1630,1.9053e-04,-0.0029,0]])

# 旋轉(zhuǎn)矩陣
R = np.array([[1.0000,1.7793e-04,-0.0041],
              [-1.6645e-04,1.0000,0.0028],
              [0.0041,-0.0028,1.0000]])
# 平移矩陣
T = np.array([-57.4647,0.5090,8.6217])

size = (1280, 720)

進行雙目校正，生成校正后的投影矩陣和重投影矩陣，通過 cv2.stereoRectify 計算得到的校正旋轉(zhuǎn)矩陣（R1、R2）和投影矩陣（P1、P2），重新映射圖像，使左右圖像對齊。使用 cv2.initUndistortRectifyMap 生成的映射表對原始圖像進行重新映射，生成查找映射表，用于將原始圖像上的點映射到校正后的圖像上。并定義鼠標回調(diào)函數(shù)，當鼠標左鍵點擊時，輸出點擊處的像素坐標及其對應的世界坐標：

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)

# --------------------------鼠標回調(diào)函數(shù)----------------------------------------------------
#   event               鼠標事件
#   param               輸入?yún)?shù)
# -----------------------------------------------------------------------------------------
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")

初始化視頻捕獲，設(shè)置窗口名稱和分辨率，并檢查攝像頭是否成功打開：

capture = cv2.VideoCapture(1)
WIN_NAME = 'Deep disp'
cv2.namedWindow(WIN_NAME, cv2.WINDOW_AUTOSIZE)
imageWidth = 1280
imageHeight = 720
print("Setting camera resolution to:", imageWidth * 2, "x", imageHeight)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, imageWidth * 2)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, imageHeight)

actual_width = capture.get(cv2.CAP_PROP_FRAME_WIDTH)
actual_height = capture.get(cv2.CAP_PROP_FRAME_HEIGHT)
print("Actual resolution set:", actual_width, "x", actual_height)

if not capture.isOpened():
    print("Error: Could not open video capture.")
    exit()

開始視頻捕獲，讀取幀并將其分割成左右圖像，然后將圖像轉(zhuǎn)換為灰度圖，進行校正并轉(zhuǎn)換回 BGR 格式：

while True:
    t1 = time.time()
    ret, frame = capture.read()
    if not ret:
        print(f"Captured no image")
    frame1 = frame[0:720, 0:1280]
    frame2 = frame[0:720, 1280:2560]
    imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
    imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)

    img1_rectified = cv2.remap(imgL, left_map1, left_map2, cv2.INTER_LINEAR)
    img2_rectified = cv2.remap(imgR, right_map1, right_map2, cv2.INTER_LINEAR)

    imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR)
    imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR)

創(chuàng)建和配置 SGBM（半全局塊匹配）算法對象并計算視差圖，對視差圖進行歸一化處理，生成灰度圖和顏色圖（之后若有空會更新SGBM算法的詳細介紹和該函數(shù)各參數(shù)含義）：

    blockSize = 3
    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)

    # 歸一化函數(shù)算法，生成深度圖（灰度圖）
    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)

計算三維坐標數(shù)據(jù)，將其乘以16以獲得實際距離。顯示深度圖和左圖像，并設(shè)置鼠標回調(diào)函數(shù)，當按下 q 鍵時退出循環(huán)：

    threeD = cv2.reprojectImageTo3D(disparity, Q, handleMissingValues=True)
    threeD = threeD * 16

    cv2.imshow("depth", dis_color)
    cv2.setMouseCallback("depth", onmouse_pick_points, threeD)

    cv2.imshow("left", frame1)
    cv2.imshow(WIN_NAME, disp)
    if cv2.waitKey(1) & 0xff == ord('q'):
        break
capture.release()
cv2.destroyAllWindows()