双目相机三维坐标计算与视差计算，python-opencv实现

原理我就不说了，就是各种公式推导和坐标转换再加上一些计算方法。

要使用这个代码首先你得有自己采集的双目图片，或者可以用cv自带的来运行代码。

其次该代码也是自己综合了许多人的代码略微调整修改了，可以说是缝合怪。其中如何匹配左右图像的点（貌似可以极线校正后利用极线匹配来做，但我好像不知道怎样做）我是自己想的，可能很粗糙，也不知道对不对。思路就是：通过给左右图片的内角点标上序号，然后左右相机的相同序号的内角点作为一组点位代入三维坐标求解函数。

直接上代码吧！我自己做的实验不理想（拍摄的图片找不到角点，图片分辨率太大不知道缩小分辨率是否会对标定有影响），但我不知道是代码问题还是啥，就发出来让大家一起学习与指导一下。

第一个版本：matlab标定，opencv处理

import numpy as np
import cv2
import os
import  xml.dom.minidom

# 遍历图片
def getFileList(dir,Filelist, ext=None):
    """
    获取文件夹及其子文件夹中文件列表
    输入 dir：文件夹根目录
    输入 ext: 扩展名
    返回： 文件路径列表
    """
    newDir = dir
    if os.path.isfile(dir):
        if ext is None:
            Filelist.append(dir)
        else:
            if ext in dir[-3:]:
                Filelist.append(dir)
    
    elif os.path.isdir(dir):
        for s in os.listdir(dir):
            newDir=os.path.join(dir,s)
            getFileList(newDir, Filelist, ext)
 
    return Filelist
# 计算内焦点世界坐标
def calRealPoint(row,col,trueLength):
    imgpoint = []
    for rowIndex in range(0,col):
        for colIndex in range(0,row):
            imgpoint.append([colIndex*trueLength,rowIndex*trueLength,0])
    imgpoint = np.array(imgpoint,np.float32)
    return imgpoint
# 画线
def draw_line1(image1, image2):
    # 建立输出图像
    height = max(image1.shape[0], image2.shape[0])
    width = image1.shape[1] + image2.shape[1]
    
    output = np.zeros((height, width,3), dtype=np.uint8)
    output[0:image1.shape[0], 0:image1.shape[1]] = image1
    output[0:image2.shape[0], image1.shape[1]:] = image2
 
    for k in range(15):
        cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA)  # 直线间隔：100
 
    return output
# 三维坐标计算
def uvToXYZ(lx, ly, rx, ry):
    mLeft = np.hstack([leftRotation, leftTranslation])
    mLeftM = np.dot(MLS, mLeft)
    mRight = np.hstack([R, T])
    mRightM = np.dot(MRS, mRight)
    # print(mLeft)
    # print(mLeftM)
    A = np.zeros(shape=(4, 3))
    for i in range(0, 3):
        A[0][i] = lx * mLeftM[2, i] - mLeftM[0][i]
    for i in range(0, 3):
        A[1][i] = ly * mLeftM[2][i] - mLeftM[1][i]
    for i in range(0, 3):
        A[2][i] = rx * mRightM[2][i] - mRightM[0][i]
    for i in range(0, 3):
        A[3][i] = ry * mRightM[2][i] - mRightM[1][i]
    B = np.zeros(shape=(4, 1))
    B[0][0] = mLeftM[0][3] - lx * mLeftM[2][3]
    B[1][0] = mLeftM[1][3] - ly * mLeftM[2][3]
    B[2][0] = mRightM[0][3] - rx * mRightM[2][3]
    B[3][0] = mRightM[1][3] - ry * mRightM[2][3]
    XYZ = np.zeros(shape=(3, 1))
    # 采用最小二乘法求其空间坐标
    cv2.solve(A, B, XYZ, cv2.DECOMP_SVD)
    # print('坐标为：\n',XYZ)
    return XYZ
# 获取像素坐标
def get_pixel(img,index):
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    _,corners = cv2.findChessboardCorners(gray,(row,col))
    criteria = (cv2.TermCriteria_EPS+cv2.TermCriteria_MAX_ITER,30,0.01)
    corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
    return corners2[index].ravel()
# 从xml(matlab生成)读取内外参数等
def getParams(file):
    dom = xml.dom.minidom.parse(file)
    root = dom.documentElement
    datas = root.getElementsByTagName('data')
    after_datas = []
    for data in datas:
        data = data.firstChild.data
        data = data.replace('\n', '').replace('\r', '')
        data = data.split(' ')[0:-1]
        data = [float(x) for x in data]
        after_datas.append(data)
    MLS = np.array(after_datas[0]).reshape(3,3)
    MRS = np.array(after_datas[1]).reshape(3,3)
    dLS = np.array(after_datas[2])
    dRS = np.array(after_datas[3])
    R = np.array(after_datas[4]).reshape(3,3)
    T = np.array(after_datas[5]).reshape(3,1) 
    return MLS,MRS,dLS,dRS,R,T 
# 清空文件
def del_file(path):
    ls = os.listdir(path)
    for i in ls:
        c_path = os.path.join(path, i)
        if os.path.isdir(c_path):
            del_file(c_path)
        else:
            os.remove(c_path)

del_file('./left_')
del_file('./right_')
del_file('./left_re')
del_file('./right_re')
leftRotation = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
leftTranslation = np.array([[0],
                            [0],
                            [0]])
# 棋盘格内焦点个数横纵
# 参数读取
MLS,MRS,dLS,dRS,R,T = getParams('2th.xml')
print('L内参\n',MLS)
print('L畸变\n',dLS)
print('R内参\n',MRS)
print('R畸变\n',dRS)
print('旋转\n',R)
print('平移\n',T)
row = 9
col = 6
# 格子真实尺寸
trueLength = 10
# 图片后缀
hz = 'jpg'
# 遍历图片
imglistL = getFileList('./left',[],hz)
imglistR = getFileList('./right',[],hz)
# 创建存储能被角点检测到的图片的文件夹
folderL = './left_'
folderR = './right_'
if not os.path.exists(folderL):
    os.mkdir(folderL)
if not os.path.exists(folderR):
    os.mkdir(folderR)
# 首先检测图片角点检测是否正确
for i in range(0,len(imglistL)):
    ChessImaR = cv2.imread(imglistR[i],0)
    ChessImaL = cv2.imread(imglistL[i],0)
    # 角点检测
    retR, cornersR = cv2.findChessboardCorners(ChessImaR,
                                               (row,col),None)
    retL, cornersL = cv2.findChessboardCorners(ChessImaL,
                                               (row,col),None)
    # 只要一对图片中有一张不能检测角点，这对图片便舍弃
    if (retR == True) & (retL == True):
        print('第%d对图片符合'%i)
        # 同时存入另一个文件夹
        cv2.imwrite(folderL+'/img'+str(i)+'.'+hz,ChessImaL)
        cv2.imwrite(folderR+'/img'+str(i)+'.'+hz,ChessImaR)
        # 画出检测的角点
        cv2.drawChessboardCorners(ChessImaL, (row, col), cornersL, retL)
        cv2.drawChessboardCorners(ChessImaR, (row, col), cornersR, retR)
        cv2.imshow('imgL',ChessImaL)
        cv2.imshow('imgR',ChessImaR)
        if cv2.waitKey(0) & 0xFF == ord(' '):
            continue
        if cv2.waitKey(0) & 0xFF == 27:
            break
    else:
        print('第%d对图片不符合,剔除'%i)
# 遍历筛选后图片
imglistL_ = getFileList(folderL,[],hz)
imglistR_ = getFileList(folderR,[],hz)
# 参数含义不知
rectify_scale = 0 
# Q重投影矩阵
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS,
                                                 ChessImaR.shape[::-1], R, T,
                                                 rectify_scale,(0,0),alpha=-1)
# 畸变校正
Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
                                             ChessImaR.shape[::-1], cv2.CV_16SC2)
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
                                              ChessImaR.shape[::-1], cv2.CV_16SC2)
# 参数设置参考官方例程
window_size = 3
min_disp = 16
num_disp = 112-min_disp
stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
    numDisparities = num_disp,
    blockSize = 16,
    P1 = 8*3*window_size**2,
    P2 = 32*3*window_size**2,
    disp12MaxDiff = 1,
    uniquenessRatio = 10,
    speckleWindowSize = 100,
    speckleRange = 32
)
# 使用滤波器降低噪声
stereoR=cv2.ximgproc.createRightMatcher(stereo)
# 过滤器参数设置
lmbda = 80000
sigma = 1.8
visual_multiplier = 1.0 
wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereo)
wls_filter.setLambda(lmbda)
wls_filter.setSigmaColor(sigma)
# 创建保存极线校正的图片
folderL_ = './left_re'
folderR_ = './right_re'
if not os.path.exists(folderL_):
    os.mkdir(folderL_)
if not os.path.exists(folderR_):
    os.mkdir(folderR_)
# 标定参数使用
for i in range(0,len(imglistL_)):
    frameR = cv2.imread(imglistR_[i])
    frameL = cv2.imread(imglistL_[i])
    # Rectify图片，极线校正
    Left_nice = cv2.remap(frameL,Left_Stereo_Map[0],Left_Stereo_Map[1], cv2.INTER_AREA)
    Right_nice = cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_AREA)
    # 保存极线校正后的图片
    cv2.imwrite(folderL_+'/img'+str(i)+'.'+hz,Left_nice)
    cv2.imwrite(folderR_+'/img'+str(i)+'.'+hz,Right_nice)
    # 显示极线校正的图片
    res1 = draw_line1(Left_nice,Right_nice)
    cv2.imshow('ln',Left_nice)
    cv2.imshow('rn',Right_nice)
    cv2.imshow('res1',res1)
    # 计算距离三维坐标
    u11,v11 = get_pixel(Left_nice,0)
    u21,v21 = get_pixel(Right_nice,0)
    u12,v12 = get_pixel(Left_nice,1)
    u22,v22 = get_pixel(Right_nice,1)
    p1 = uvToXYZ(u11,v11,u21,v21)
    p2 = uvToXYZ(u12,v12,u22,v22)
    print('p1坐标\n',p1)
    print('p2坐标\n',p2)
    print('距离\n',np.linalg.norm(p1-p2))
    ################################################
    grayR= cv2.cvtColor(Right_nice,cv2.COLOR_BGR2GRAY)
    grayL= cv2.cvtColor(Left_nice,cv2.COLOR_BGR2GRAY)
    # 计算深度，参考官方例程
    dispL= stereo.compute(grayL,grayR).astype(np.float32)/ 16.0 
    dispR= stereo.compute(grayR,grayL).astype(np.float32)/ 16.0
    # 使用滤波器降噪,filteredimg为过滤后的深度图
    filteredImg= wls_filter.filter(dispL,grayL,None,dispR)
    filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, 
                                norm_type=cv2.NORM_MINMAX)
    filteredImg = np.uint8(filteredImg)
    
    points = cv2.reprojectImageTo3D(dispL,Q)

    cv2.imshow('filteredImg',filteredImg)
    cv2.imshow('disparity', (dispL-min_disp)/num_disp)
    
    if cv2.waitKey(0) & 0xFF == ord(' '):
        continue
    if cv2.waitKey(0) & 0xFF == 27:
        break

cv2.waitKey(0)
cv2.destroyAllWindows()

matlab标定参数变xml

function writeXML(stereoParams,file)

docNode = com.mathworks.xml.XMLUtils.createDocument('opencv_storage'); %创建xml文件对象
docRootNode = docNode.getDocumentElement; %获取根节点

IntrinsicMatrixl = (stereoParams.CameraParameters1.IntrinsicMatrix)'; %相机内参矩阵
RadialDistortionl = stereoParams.CameraParameters1.RadialDistortion; %相机径向畸变参数向量1*3
TangentialDistortionl =stereoParams.CameraParameters1.TangentialDistortion; %相机切向畸变向量1*2
Distortionl = [RadialDistortionl(1:2),TangentialDistortionl,RadialDistortionl(3)]; %构成opencv中的畸变系数向量[k1,k2,p1,p2,k3]

IntrinsicMatrixr = (stereoParams.CameraParameters2.IntrinsicMatrix)'; %相机内参矩阵
RadialDistortionr = stereoParams.CameraParameters2.RadialDistortion; %相机径向畸变参数向量1*3
TangentialDistortionr =stereoParams.CameraParameters2.TangentialDistortion; %相机切向畸变向量1*2
Distortionr = [RadialDistortionr(1:2),TangentialDistortionr,RadialDistortionr(3)]; %构成opencv中的畸变系数向量[k1,k2,p1,p2,k3]

R = stereoParams.RotationOfCamera2';
T = stereoParams.TranslationOfCamera2;

camera_matrix = docNode.createElement('lcamera-matrix'); %创建mat节点
data = docNode.createElement('data');
for i=1:3
    for j=1:3
        data.appendChild(docNode.createTextNode(sprintf('%.16f ',IntrinsicMatrixl(i,j))));
    end
    data.appendChild(docNode.createTextNode(sprintf('\n')));
end
camera_matrix.appendChild(data);
docRootNode.appendChild(camera_matrix);

camera_matrix = docNode.createElement('rcamera-matrix'); %创建mat节点
data = docNode.createElement('data');
for i=1:3
    for j=1:3
        data.appendChild(docNode.createTextNode(sprintf('%.16f ',IntrinsicMatrixr(i,j))));
    end
    data.appendChild(docNode.createTextNode(sprintf('\n')));
end
camera_matrix.appendChild(data);
docRootNode.appendChild(camera_matrix);

distortion = docNode.createElement('ldistortion');
data = docNode.createElement('data');
for i=1:5
      data.appendChild(docNode.createTextNode(sprintf('%.16f ',Distortionl(i))));
end
distortion.appendChild(data);
docRootNode.appendChild(distortion);

distortion = docNode.createElement('rdistortion');
data = docNode.createElement('data');
for i=1:5
      data.appendChild(docNode.createTextNode(sprintf('%.16f ',Distortionr(i))));
end
distortion.appendChild(data);
docRootNode.appendChild(distortion);

rM = docNode.createElement('R');
data = docNode.createElement('data');
for i=1:3
    for j=1:3
        data.appendChild(docNode.createTextNode(sprintf('%.16f ',R(i,j))));
    end
    data.appendChild(docNode.createTextNode(sprintf('\n')));
end
rM.appendChild(data);
docRootNode.appendChild(rM);

tM = docNode.createElement('T');
data = docNode.createElement('data');
for i = 1:3
    data.appendChild(docNode.createTextNode(sprintf('%.16f ',T(i))));
end
tM.appendChild(data);
docRootNode.appendChild(tM);

xmlFileName = file;
xmlwrite(xmlFileName,docNode);
end

第二个版本，标定与立体校正匹配等全用cv

 

import numpy as np
import cv2
import os

# 遍历图片
def getFileList(dir,Filelist, ext=None):
    """
    获取文件夹及其子文件夹中文件列表
    输入 dir：文件夹根目录
    输入 ext: 扩展名
    返回： 文件路径列表
    """
    newDir = dir
    if os.path.isfile(dir):
        if ext is None:
            Filelist.append(dir)
        else:
            if ext in dir[-3:]:
                Filelist.append(dir)
    
    elif os.path.isdir(dir):
        for s in os.listdir(dir):
            newDir=os.path.join(dir,s)
            getFileList(newDir, Filelist, ext)
 
    return Filelist
# 计算内焦点世界坐标
def calRealPoint(row,col,trueLength):
    imgpoint = []
    for rowIndex in range(0,col):
        for colIndex in range(0,row):
            imgpoint.append([colIndex*trueLength,rowIndex*trueLength,0])
    imgpoint = np.array(imgpoint,np.float32)
    return imgpoint
# 画线
def draw_line1(image1, image2):
    # 建立输出图像
    height = max(image1.shape[0], image2.shape[0])
    width = image1.shape[1] + image2.shape[1]
    
    output = np.zeros((height, width,3), dtype=np.uint8)
    output[0:image1.shape[0], 0:image1.shape[1]] = image1
    output[0:image2.shape[0], image1.shape[1]:] = image2
 
    for k in range(15):
        cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA)  # 直线间隔：100
 
    return output
# 三维坐标计算
def uvToXYZ(lx, ly, rx, ry):
    mLeft = np.hstack([leftRotation, leftTranslation])
    mLeftM = np.dot(MLS, mLeft)
    mRight = np.hstack([R, T])
    mRightM = np.dot(MRS, mRight)
    # print(mLeft)
    # print(mLeftM)
    A = np.zeros(shape=(4, 3))
    for i in range(0, 3):
        A[0][i] = lx * mLeftM[2, i] - mLeftM[0][i]
    for i in range(0, 3):
        A[1][i] = ly * mLeftM[2][i] - mLeftM[1][i]
    for i in range(0, 3):
        A[2][i] = rx * mRightM[2][i] - mRightM[0][i]
    for i in range(0, 3):
        A[3][i] = ry * mRightM[2][i] - mRightM[1][i]
    B = np.zeros(shape=(4, 1))
    B[0][0] = mLeftM[0][3] - lx * mLeftM[2][3]
    B[1][0] = mLeftM[1][3] - ly * mLeftM[2][3]
    B[2][0] = mRightM[0][3] - rx * mRightM[2][3]
    B[3][0] = mRightM[1][3] - ry * mRightM[2][3]
    XYZ = np.zeros(shape=(3, 1))
    # 采用最小二乘法求其空间坐标
    cv2.solve(A, B, XYZ, cv2.DECOMP_SVD)
    # print('坐标为：\n',XYZ)
    return XYZ
# 获取像素坐标
def get_pixel(img,index):
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    _,corners = cv2.findChessboardCorners(gray,(row,col))
    criteria = (cv2.TermCriteria_EPS+cv2.TermCriteria_MAX_ITER,30,0.01)
    corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
    return corners2[index].ravel()
# 清空文件夹文件
def del_file(path):
    ls = os.listdir(path)
    for i in ls:
        c_path = os.path.join(path, i)
        if os.path.isdir(c_path):
            del_file(c_path)
        else:
            os.remove(c_path)

# 初始化
del_file('./left_')
del_file('./right_')
del_file('./left_re')
del_file('./right_re')
leftRotation = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
leftTranslation = np.array([[0],
                            [0],
                            [0]])
# 棋盘格内焦点个数横纵
row = 9
col = 6
# 格子真实尺寸
trueLength = 10
# 图片后缀
hz = 'jpg'
# 遍历图片
imglistL = getFileList('./left',[],hz)
imglistR = getFileList('./right',[],hz)
# 创建存储能被角点检测到的图片的文件夹
folderL = './left_'
folderR = './right_'
if not os.path.exists(folderL):
    os.mkdir(folderL)
if not os.path.exists(folderR):
    os.mkdir(folderR)
# 首先检测图片角点检测是否正确
for i in range(0,len(imglistL)):
    ChessImaR = cv2.imread(imglistR[i],0)
    ChessImaL = cv2.imread(imglistL[i],0)
    # 角点检测
    retR, cornersR = cv2.findChessboardCorners(ChessImaR,
                                               (row,col),None)
    retL, cornersL = cv2.findChessboardCorners(ChessImaL,
                                               (row,col),None)
    # 只要一对图片中有一张不能检测角点，这对图片便舍弃
    if (retR == True) & (retL == True):
        print('第%d对图片符合'%i)
        # 同时存入另一个文件夹
        cv2.imwrite(folderL+'/img'+str(i)+'.'+hz,ChessImaL)
        cv2.imwrite(folderR+'/img'+str(i)+'.'+hz,ChessImaR)
        # 画出检测的角点
        cv2.drawChessboardCorners(ChessImaL, (row, col), cornersL, retL)
        cv2.drawChessboardCorners(ChessImaR, (row, col), cornersR, retR)
        cv2.imshow('imgL',ChessImaL)
        cv2.imshow('imgR',ChessImaR)
        if cv2.waitKey(0) & 0xFF == ord(' '):
            continue
        if cv2.waitKey(0) & 0xFF == 27:
            break
    else:
        print('第%d对图片不符合,剔除'%i)
#############################################################
#############################################################
# 对筛选后的图像进行标定，校正，匹配，求视差，最小二乘求解空间坐标
# 亚像素的参数和立体标定的参数
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
criteria_stereo = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# 遍历筛选后图片
imglistL_ = getFileList(folderL,[],hz)
imglistR_ = getFileList(folderR,[],hz)
# 存储每对图片的世界坐标，z为0，其它根据格子实际长度计算
objpoints = []
# 分别存储左右图片的角点亚像素坐标   
imgpointsR = []
imgpointsL = []
# 计算世界坐标下内角的坐标
objp = calRealPoint(row,col,trueLength)

print('Starting calibration for the 2 cameras... ')
for i in range(0,len(imglistL_)):   
    ChessImaL_ = cv2.imread(imglistL_[i],0) 
    ChessImaR_ = cv2.imread(imglistR_[i],0)   
    retR_, cornersR_ = cv2.findChessboardCorners(ChessImaR_,
                                               (row,col),None) 
    retL_, cornersL_ = cv2.findChessboardCorners(ChessImaL_,
                                               (row,col),None)
    if (True == retR_) & (True == retL_):
        # 亚像素
        cornersR2_ = cv2.cornerSubPix(ChessImaR_,cornersR_,(11,11),(-1,-1),criteria)
        cornersL2_ = cv2.cornerSubPix(ChessImaL_,cornersL_,(11,11),(-1,-1),criteria)
        imgpointsR.append(cornersR2_)
        imgpointsL.append(cornersL2_)
        objpoints.append(objp)
        # 画角点
        cv2.drawChessboardCorners(ChessImaL_, (row, col), cornersL2_, retL_)
        cv2.drawChessboardCorners(ChessImaR_, (row, col), cornersR2_, retR_)
        print('第%d对图片'%i)
        cv2.imshow('imgL',ChessImaL_)
        cv2.imshow('imgR',ChessImaR_)
        if cv2.waitKey(0) & 0xFF == ord(' '):
            continue
        if cv2.waitKey(0) & 0xFF == 27:
            break
# 类似先进行单目标定
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(objpoints,
                                                        imgpointsR,
                                                        ChessImaR_.shape[::-1],None,None)
# 重投影误差计算
for i in range(0,len(objpoints)):
    err = 0
    ChessImaR_1 = cv2.imread(imglistR_[i])
    # 反向计算像素坐标，与亚像素角点坐标比较 ,计算的temp有两部分
    temp = cv2.projectPoints(objpoints[i],rvecsR[i],tvecsR[i],mtxR,distR)
    for j in range(0,temp[0].shape[0]):
        err = err+np.linalg.norm(temp[0][j]-imgpointsR[i][j])
    err = err/temp[0].shape[0]
    print('右边第%d张重新投影与原角点亚像素平均误差为:%f\n'%(i,err))
    cv2.drawChessboardCorners(ChessImaR_1, (row, col), temp[0],patternWasFound=True)
    cv2.imshow('the_right',ChessImaR_1)
    if cv2.waitKey(0) & 0xFF == ord(' '):
        continue
    if cv2.waitKey(0) & 0xFF == 27:
        break
###############################################################################
retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(objpoints,
                                                        imgpointsL,
                                                        ChessImaL_.shape[::-1],None,None)
# 重投影误差计算
for i in range(0,len(objpoints)):
    err = 0
    ChessImaL_1 = cv2.imread(imglistL_[i])
    # 反向计算像素坐标，与亚像素角点坐标比较 ,计算的temp有两部分
    temp = cv2.projectPoints(objpoints[i],rvecsL[i],tvecsL[i],mtxL,distL)
    for j in range(0,temp[0].shape[0]):
        err = err+np.linalg.norm(temp[0][j]-imgpointsL[i][j])
    err = err/temp[0].shape[0]
    print('左边第%d张重新投影与原角点亚像素平均误差为:%f\n'%(i,err))
    cv2.drawChessboardCorners(ChessImaL_1, (row, col), temp[0],patternWasFound=True)
    cv2.imshow('the_left',ChessImaL_1)
    if cv2.waitKey(0) & 0xFF == ord(' '):
        continue
    if cv2.waitKey(0) & 0xFF == 27:
        break
print('Cameras Ready to use')
#############################################################################
# 立体标定及可选参数，具体查阅
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
#flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_USE_INTRINSIC_GUESS
#flags |= cv2.CALIB_FIX_FOCAL_LENGTH
#flags |= cv2.CALIB_FIX_ASPECT_RATIO
#flags |= cv2.CALIB_ZERO_TANGENT_DIST
#flags |= cv2.CALIB_RATIONAL_MODEL
#flags |= cv2.CALIB_SAME_FOCAL_LENGTH
#flags |= cv2.CALIB_FIX_K3
#flags |= cv2.CALIB_FIX_K4
#flags |= cv2.CALIB_FIX_K5
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(objpoints,
                                                          imgpointsL,
                                                          imgpointsR,
                                                          mtxL,
                                                          distL,
                                                          mtxR,
                                                          distR,
                                                          ChessImaR.shape[::-1],
                                                          criteria_stereo,
                                                          flags)
print('L内参\n',MLS)
print('L畸变\n',dLS)
print('R内参\n',MRS)
print('R畸变\n',dRS)
print('旋转\n',R)
print('平移\n',T)
# 参数含义不知
rectify_scale = 0 
# Q重投影矩阵
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS,
                                                 ChessImaR.shape[::-1], R, T,
                                                 rectify_scale,(0,0),alpha=-1)
# 畸变校正
Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
                                             ChessImaR.shape[::-1], cv2.CV_16SC2)
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
                                              ChessImaR.shape[::-1], cv2.CV_16SC2)
# 参数设置参考官方例程
window_size = 3
min_disp = 16
num_disp = 112-min_disp
stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
    numDisparities = num_disp,
    blockSize = 16,
    P1 = 8*3*window_size**2,
    P2 = 32*3*window_size**2,
    disp12MaxDiff = 1,
    uniquenessRatio = 10,
    speckleWindowSize = 100,
    speckleRange = 32
)
# 使用滤波器降低噪声
stereoR=cv2.ximgproc.createRightMatcher(stereo)
# 过滤器参数设置
lmbda = 80000
sigma = 1.8
visual_multiplier = 1.0 
wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereo)
wls_filter.setLambda(lmbda)
wls_filter.setSigmaColor(sigma)
# 创建保存极线校正的图片
folderL_ = './left_re'
folderR_ = './right_re'
if not os.path.exists(folderL_):
    os.mkdir(folderL_)
if not os.path.exists(folderR_):
    os.mkdir(folderR_)
# 标定参数使用
for i in range(0,len(imglistL_)):
    frameR = cv2.imread(imglistR_[i])
    frameL = cv2.imread(imglistL_[i])
    # Rectify图片，极线校正
    Left_nice = cv2.remap(frameL,Left_Stereo_Map[0],Left_Stereo_Map[1], cv2.INTER_AREA)
    Right_nice = cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_AREA)
    # 保存极线校正后的图片
    cv2.imwrite(folderL_+'/img'+str(i)+'.'+hz,Left_nice)
    cv2.imwrite(folderR_+'/img'+str(i)+'.'+hz,Right_nice)
    # 显示极线校正的图片
    res1 = draw_line1(Left_nice,Right_nice)
    cv2.imshow('ln',Left_nice)
    cv2.imshow('rn',Right_nice)
    cv2.imshow('res1',res1)
    # 计算距离三维坐标
    u11,v11 = get_pixel(Left_nice,0)
    u21,v21 = get_pixel(Right_nice,0)
    u12,v12 = get_pixel(Left_nice,1)
    u22,v22 = get_pixel(Right_nice,1)
    p1 = uvToXYZ(u11,v11,u21,v21)
    p2 = uvToXYZ(u12,v12,u22,v22)
    print('p1坐标\n',p1)
    print('p2坐标\n',p2)
    print('距离\n',np.linalg.norm(p1-p2))
    ################################################
    grayR= cv2.cvtColor(Right_nice,cv2.COLOR_BGR2GRAY)
    grayL= cv2.cvtColor(Left_nice,cv2.COLOR_BGR2GRAY)
    # 计算深度，参考官方例程
    dispL= stereo.compute(grayL,grayR).astype(np.float32)/ 16.0 
    dispR= stereo.compute(grayR,grayL).astype(np.float32)/ 16.0
    # 使用滤波器降噪,filteredimg为过滤后的深度图
    filteredImg= wls_filter.filter(dispL,grayL,None,dispR)
    filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, 
                                norm_type=cv2.NORM_MINMAX)
    filteredImg = np.uint8(filteredImg)

    points = cv2.reprojectImageTo3D(dispL,Q)

    cv2.imshow('filteredImg',filteredImg)
    cv2.imshow('disparity', (dispL-min_disp)/num_disp)
    
    if cv2.waitKey(0) & 0xFF == ord(' '):
        continue
    if cv2.waitKey(0) & 0xFF == 27:
        break

cv2.waitKey(0)
cv2.destroyAllWindows()

希望能帮到大家，最好有大佬能帮我解决一个问题，以上代码都是网上搜集自己整理，我也不知道是谁的了，就感谢大家所有人！