Stereo Distance measurement by SGM. :star: it if you like. :D
﻿#      ▄▀▄     ▄▀▄
#     ▄█░░▀▀▀▀▀░░█▄
# ▄▄  █░░░░░░░░░░░█  ▄▄
#█▄▄█ █░░▀░░┬░░▀░░█ █▄▄█

###################################
##### Authors:                #####
##### Stephane Vujasinovic    #####
##### Frederic Uhrweiller     ##### 
#####                         #####
##### Creation: 2017          #####
###################################


#***********************
#**** Main Programm ****
#***********************


# Package importation
import numpy as np
import cv2
from openpyxl import Workbook # Used for writing data into an Excel file
from sklearn.preprocessing import normalize

# Filtering
kernel= np.ones((3,3),np.uint8)

def coords_mouse_disp(event,x,y,flags,param):
    if event == cv2.EVENT_LBUTTONDBLCLK:
        #print x,y,disp[y,x],filteredImg[y,x]
        average=0
        for u in range (-1,2):
            for v in range (-1,2):
                average += disp[y+u,x+v]
        average=average/9
        Distance= -593.97*average**(3) + 1506.8*average**(2) - 1373.1*average + 522.06
        Distance= np.around(Distance*0.01,decimals=2)
        print('Distance: '+ str(Distance)+' m')
        
# This section has to be uncommented if you want to take mesurements and store them in the excel
##        ws.append([counterdist, average])
##        print('Measure at '+str(counterdist)+' cm, the dispasrity is ' + str(average))
##        if (counterdist <= 85):
##            counterdist += 3
##        elif(counterdist <= 120):
##            counterdist += 5
##        else:
##            counterdist += 10
##        print('Next distance to measure: '+str(counterdist)+'cm')

# Mouseclick callback
wb=Workbook()
ws=wb.active  

#*************************************************
#***** Parameters for Distortion Calibration *****
#*************************************************

# Termination criteria
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)

# Prepare object points
objp = np.zeros((9*6,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)

# Arrays to store object points and image points from all images
objpoints= []   # 3d points in real world space
imgpointsR= []   # 2d points in image plane
imgpointsL= []

# Start calibration from the camera
print('Starting calibration for the 2 cameras... ')
# Call all saved images
for i in range(0,67):   # Put the amount of pictures you have taken for the calibration inbetween range(0,?) wenn starting from the image number 0
    t= str(i)
    ChessImaR= cv2.imread('chessboard-R'+t+'.png',0)    # Right side
    ChessImaL= cv2.imread('chessboard-L'+t+'.png',0)    # Left side
    retR, cornersR = cv2.findChessboardCorners(ChessImaR,
                                               (9,6),None)  # Define the number of chees corners we are looking for
    retL, cornersL = cv2.findChessboardCorners(ChessImaL,
                                               (9,6),None)  # Left side
    if (True == retR) & (True == retL):
        objpoints.append(objp)
        cv2.cornerSubPix(ChessImaR,cornersR,(11,11),(-1,-1),criteria)
        cv2.cornerSubPix(ChessImaL,cornersL,(11,11),(-1,-1),criteria)
        imgpointsR.append(cornersR)
        imgpointsL.append(cornersL)

# Determine the new values for different parameters
#   Right Side
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(objpoints,
                                                        imgpointsR,
                                                        ChessImaR.shape[::-1],None,None)
hR,wR= ChessImaR.shape[:2]
OmtxR, roiR= cv2.getOptimalNewCameraMatrix(mtxR,distR,
                                                   (wR,hR),1,(wR,hR))

#   Left Side
retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(objpoints,
                                                        imgpointsL,
                                                        ChessImaL.shape[::-1],None,None)
hL,wL= ChessImaL.shape[:2]
OmtxL, roiL= cv2.getOptimalNewCameraMatrix(mtxL,distL,(wL,hL),1,(wL,hL))

print('Cameras Ready to use')

#********************************************
#***** Calibrate the Cameras for Stereo *****
#********************************************

# StereoCalibrate function
#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 = criteria_stereo,
                                                          flags = cv2.CALIB_FIX_INTRINSIC)

# StereoRectify function
rectify_scale= 0 # if 0 image croped, if 1 image nor croped
RL, RR, PL, PR, Q, roiL, roiR= cv2.stereoRectify(MLS, dLS, MRS, dRS,
                                                 ChessImaR.shape[::-1], R, T,
                                                 rectify_scale,(0,0))  # last paramater is alpha, if 0= croped, if 1= not croped
# initUndistortRectifyMap function
Left_Stereo_Map= cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
                                             ChessImaR.shape[::-1], cv2.CV_16SC2)   # cv2.CV_16SC2 this format enables us the programme to work faster
Right_Stereo_Map= cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
                                              ChessImaR.shape[::-1], cv2.CV_16SC2)
#*******************************************
#***** Parameters for the StereoVision *****
#*******************************************

# Create StereoSGBM and prepare all parameters
window_size = 3
min_disp = 2
num_disp = 130-min_disp
stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
    numDisparities = num_disp,
    blockSize = window_size,
    uniquenessRatio = 10,
    speckleWindowSize = 100,
    speckleRange = 32,
    disp12MaxDiff = 5,
    P1 = 8*3*window_size**2,
    P2 = 32*3*window_size**2)

# Used for the filtered image
stereoR=cv2.ximgproc.createRightMatcher(stereo) # Create another stereo for right this time

# WLS FILTER Parameters
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)

#*************************************
#***** Starting the StereoVision *****
#*************************************

# Call the two cameras
CamR= cv2.VideoCapture(0)   # Wenn 0 then Right Cam and wenn 2 Left Cam
CamL= cv2.VideoCapture(2)

while True:
    # Start Reading Camera images
    retR, frameR= CamR.read()
    retL, frameL= CamL.read()

    # Rectify the images on rotation and alignement
    Left_nice= cv2.remap(frameL,Left_Stereo_Map[0],Left_Stereo_Map[1], interpolation = cv2.INTER_LANCZOS4, borderMode = cv2.BORDER_CONSTANT)  # Rectify the image using the kalibration parameters founds during the initialisation
    Right_nice= cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], interpolation = cv2.INTER_LANCZOS4, borderMode = cv2.BORDER_CONSTANT)

##    # Draw Red lines
##    for line in range(0, int(Right_nice.shape[0]/20)): # Draw the Lines on the images Then numer of line is defines by the image Size/20
##        Left_nice[line*20,:]= (0,0,255)
##        Right_nice[line*20,:]= (0,0,255)
##
##    for line in range(0, int(frameR.shape[0]/20)): # Draw the Lines on the images Then numer of line is defines by the image Size/20
##        frameL[line*20,:]= (0,255,0)
##        frameR[line*20,:]= (0,255,0)    
        
    # Show the Undistorted images
    #cv2.imshow('Both Images', np.hstack([Left_nice, Right_nice]))
    #cv2.imshow('Normal', np.hstack([frameL, frameR]))

    # Convert from color(BGR) to gray
    grayR= cv2.cvtColor(Right_nice,cv2.COLOR_BGR2GRAY)
    grayL= cv2.cvtColor(Left_nice,cv2.COLOR_BGR2GRAY)

    # Compute the 2 images for the Depth_image
    disp= stereo.compute(grayL,grayR)#.astype(np.float32)/ 16
    dispL= disp
    dispR= stereoR.compute(grayR,grayL)
    dispL= np.int16(dispL)
    dispR= np.int16(dispR)

    # Using the WLS filter
    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)
    #cv2.imshow('Disparity Map', filteredImg)
    disp= ((disp.astype(np.float32)/ 16)-min_disp)/num_disp # Calculation allowing us to have 0 for the most distant object able to detect

##    # Resize the image for faster executions
##    dispR= cv2.resize(disp,None,fx=0.7, fy=0.7, interpolation = cv2.INTER_AREA)

    # Filtering the Results with a closing filter
    closing= cv2.morphologyEx(disp,cv2.MORPH_CLOSE, kernel) # Apply an morphological filter for closing little "black" holes in the picture(Remove noise) 

    # Colors map
    dispc= (closing-closing.min())*255
    dispC= dispc.astype(np.uint8)                                   # Convert the type of the matrix from float32 to uint8, this way you can show the results with the function cv2.imshow()
    disp_Color= cv2.applyColorMap(dispC,cv2.COLORMAP_OCEAN)         # Change the Color of the Picture into an Ocean Color_Map
    filt_Color= cv2.applyColorMap(filteredImg,cv2.COLORMAP_OCEAN) 

    # Show the result for the Depth_image
    #cv2.imshow('Disparity', disp)
    #cv2.imshow('Closing',closing)
    #cv2.imshow('Color Depth',disp_Color)
    cv2.imshow('Filtered Color Depth',filt_Color)

    # Mouse click
    cv2.setMouseCallback("Filtered Color Depth",coords_mouse_disp,filt_Color)
    
    # End the Programme
    if cv2.waitKey(1) & 0xFF == ord(' '):
        break
    
# Save excel
##wb.save("data4.xlsx")

# Release the Cameras
CamR.release()
CamL.release()
cv2.destroyAllWindows()

Main_Stereo_Vision_Prog.py
jiayuzhang128/StereoMeasurement
import cv2
import glob
import sys


def saveCoefficients(path, mtx, dist, rms):
    """ Save the camera matrix and the distortion coefficients to given path/file. """
    cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE)
    cv_file.write("K", mtx)
    cv_file.write("D", dist)
    cv_file.write("RMS", rms)
    # note you *release* you don't close() a FileStorage object
    cv_file.release()


def saveStereoCoefficients(path, imageSize, K1, D1, K2, D2, R, T, E, F, rms, R1=None, R2=None, P1=None, P2=None, Q=None, roiL=None, roiR=None, mapLx=None, mapLy=None, mapRx=None, mapRy=None):
    """ Save the stereo coefficients to given path/file. """
    cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE)
    cv_file.write("Size", imageSize)
    cv_file.write("K1", K1)
    cv_file.write("D1", D1)
    cv_file.write("K2", K2)
    cv_file.write("D2", D2)
    cv_file.write("R", R)
    cv_file.write("T", T)
    cv_file.write("E", E)
    cv_file.write("F", F)
    cv_file.write("RMS", rms)
    if all(i is not None for i in [R1, R2, P1, P2, Q, roiL, roiR, mapLx, mapLy, mapRx, mapRy]):
        cv_file.write("R1", R1)
        cv_file.write("R2", R2)
        cv_file.write("P1", P1)
        cv_file.write("P2", P2)
        cv_file.write("Q", Q)
        cv_file.write("ROIL", roiL)
        cv_file.write("ROIR", roiR)
        cv_file.write("MAPLX", mapLx)
        cv_file.write("MAPLY", mapLy)
        cv_file.write("MAPRX", mapRx)
        cv_file.write("MAPRY", mapRy)
    cv_file.release()


def loadCoefficients(path):
    """ Loads camera matrix and distortion coefficients. """
    # FILE_STORAGE_READ
    cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_READ)

    # note we also have to specify the type to retrieve other wise we only get a
    # FileNode object back instead of a matrix
    camera_matrix = cv_file.getNode("K").mat()
    dist_matrix = cv_file.getNode("D").mat()

    cv_file.release()
    return [camera_matrix, dist_matrix]


def loadStereoCoefficients(path, rectifid=False):
    """ Loads stereo matrix coefficients. """
    # 读取文件
    cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_READ)

    imageSize = cv_file.getNode("Size").mat()
    Size = tuple([int(imageSize[1,0]),int(imageSize[0,0])])
    K1 = cv_file.getNode("K1").mat()
    D1 = cv_file.getNode("D1").mat()
    K2 = cv_file.getNode("K2").mat()
    D2 = cv_file.getNode("D2").mat()
    R = cv_file.getNode("R").mat()
    T = cv_file.getNode("T").mat()
    E = cv_file.getNode("E").mat()
    F = cv_file.getNode("F").mat()
    rms = cv_file.getNode("RMS").real()
    if rectifid == True:
        R1 = cv_file.getNode("R1").mat()
        R2 = cv_file.getNode("R2").mat()
        P1 = cv_file.getNode("P1").mat()
        P2 = cv_file.getNode("P2").mat()
        Q = cv_file.getNode("Q").mat()
        roiL = cv_file.getNode("ROIL").mat()
        roiR = cv_file.getNode("ROIR").mat()
        mapLx = cv_file.getNode("MAPLX").mat()
        mapLy = cv_file.getNode("MAPLY").mat()
        mapRx = cv_file.getNode("MAPRX").mat()
        mapRy = cv_file.getNode("MAPRY").mat()
        result = [Size, K1, D1, K2, D2, R, T, E, F, rms, R1, R2, P1, P2, Q, roiL, roiR, mapLx, mapLy, mapRx, mapRy]
    else:
        result = [Size, K1, D1, K2, D2, R, T, E, F, rms]

    cv_file.release()
    return result

def loadStereoImages(dirL, dirR, imageFormat):

    # 图像路径校正. 移除'/'
    if dirL[-1:] == '/':
        dirL = dirL[:-1]
    if dirR[-1:] == '/':
        dirR = dirR[:-1]

    # 获取该路径下的所有图片
    imagesL = glob.glob(dirL + '/' + '*.' + imageFormat)
    imagesR = glob.glob(dirR + '/' + '*.' + imageFormat)
    imagesL.sort()
    imagesR.sort()

    # 确认找到图片
    if len(imagesL) & len(imagesR) == False:
        print("未找到图片，请检查路径后重新加载!")

    # 确保图片数量一致
    if len(imagesL) != len(imagesR):
        print("左图数量: ", len(imagesL))
        print("右图数量: ", len(imagesR))
        print("左右图像不均等，请检查路径后重新加载！")
        sys.exit(-1)
    
    imagesLR = zip(imagesL,imagesR)
    return imagesLR

calibrationStore.py
jiayuzhang128/StereoMeasurement
#  ==================================================================================
#  作   者：张佳预
#  日   期：2023/1/13
#  版   本：v1.0
#  代码描述：用于获取单目相机或双目相机的标定图片
#  软件平台：python==3.7.5
#  运   行:
#      直接点击即可运行
#  ==================================================================================

import cv2
import os


def makeDir(numCam=1, path='CalibData'):
    '''
    创建用于存放标定图片的文件夹\n
    参数：\n
        numCam：相机数量，默认为1\n
        path：文件夹名称，默认为‘ClibData’
    '''
    if numCam == 1:
        path += 'Mono'
        folder = os.path.exists(path)
        if not folder:
            os.makedirs(path)
            print(path, ' 文件夹创建成功')
        else:
            print(path, ' 文件夹已经存在')
        return path
    elif numCam == 2:
        path += 'Stereo'
        folder = os.path.exists(path)
        if not folder:
            os.makedirs(path)
            os.makedirs(path + r'/left')
            os.makedirs(path + r'/right')
            print(path, ' 文件夹创建成功')
            print(path + r'/left', ' 文件夹创建成功')
            print(path + r'/right', ' 文件夹创建成功')
            return path
        else:
            print(path, ' 文件夹已经存在')
        return path
    else:
        print("文件夹创建失败！请将摄像头数量numCam设置为1或2！")
        return 0


def getPicture(numCam, index, savePath, width=640, height=480, criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001), numCorner1=11, numCorner2=8, idImage=0):
    '''
    拍摄相机标定图像\n
    参数：\n
    numCam：相机数量
    index：相机索引\n
    savePath：图片保存路径\n
    width：图片宽
    height：图片高
    criteria：获得亚像素角点算法终止条件，默认为‘最大迭代30次或误差小于0.001时终止’\n
    numCorner1：棋盘格行角点数，默认为8\n
    numCorner2：棋盘格行角点数，默认为5\n
    idImage：保存图片的序号
    '''
    # 调用摄像头
    cap = cv2.VideoCapture(index)                # index -> 摄像头索引
    # 设置视频流属性，一般不要随意调整相机属性
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)     # 宽度
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)   # 高度
    # cap.set(cv2.CAP_PROP_AUTO_WB, 1)             # 自动白平衡
    # cap.set(cv2.CAP_PROP_FPS, 30)                # 帧率30
    # cap.set(cv2.CAP_PROP_BRIGHTNESS, 0)          # 亮度0
    # cap.set(cv2.CAP_PROP_CONTRAST, 0)            # 对比度0
    # cap.set(cv2.CAP_PROP_SATURATION, 36)         # 饱和度36
    # cap.set(cv2.CAP_PROP_HUE, 0)                 # 色调0
    # cap.set(cv2.CAP_PROP_EXPOSURE, -6)           # 曝光-6


    if numCam == 2:
        # 定义显示窗口
        cv2.namedWindow("Video2",cv2.WINDOW_NORMAL)
        while True:
            ret, frame = cap.read()
            frameL = frame[0:720,0:1280]
            frameR = frame[0:720,1280:2560]
            camL = frameL.copy()
            camR = frameR.copy()
            grayL = cv2.cvtColor(camL, cv2.COLOR_BGR2GRAY)
            grayR = cv2.cvtColor(camR, cv2.COLOR_BGR2GRAY)

            # 寻找棋盘格角点
            retL, cornersL = cv2.findChessboardCorners(grayL, (numCorner1, numCorner2), None)
            retR, cornersR = cv2.findChessboardCorners(grayR, (numCorner1, numCorner2), None)

            if retL == True & retR == True:
                # 获得亚像素角点坐标
                corners2L = cv2.cornerSubPix(grayL, cornersL, (11, 11), (-1, -1), criteria)
                corners2R = cv2.cornerSubPix(grayR, cornersR, (11, 11), (-1, -1), criteria)
                # 绘制并显示角点
                cv2.drawChessboardCorners(camL, (numCorner1, numCorner2), corners2L, retL)
                cv2.drawChessboardCorners(camR, (numCorner1, numCorner2), corners2R, retR)
            # 两个框合并
            camLR = cv2.hconcat([camL,camR])
            cv2.imshow('Video2', camLR)
            # cv2.imshow('VideoL', camL)
            # cv2.imshow('VideoR', camR)

            key = cv2.waitKey(1)
            # 按‘s’健保存图片
            if (key & 0xFF == ord('s')):
                if retL == True & retL == True:
                    strIdImage = str(idImage)
                    cv2.imwrite(savePath + '/left/' + strIdImage + '.png', frameL)
                    cv2.imwrite(savePath + '/right/' + strIdImage + '.png', frameR)
                    print('第{}张图片，保存成功'.format(idImage))
                    idImage = idImage+1
                else:
                    print('保存失败！棋盘格不完整，请换个角度重新保存！')

            # 按'ESC'退出程序
            elif key & 0xFF == 27:
                print('程序已终止！一共保存了{}张图片'.format(idImage))
                break

    elif numCam == 1:
        cv2.namedWindow("Video1",cv2.WINDOW_NORMAL)
        while True:
            ret, frame = cap.read()
            cam = frame.copy()
            gray = cv2.cvtColor(cam, cv2.COLOR_BGR2GRAY)
            # 寻找棋盘格角点
            rets, corners = cv2.findChessboardCorners(gray, (numCorner1, numCorner2), None)

            if rets == True:
                # 获得亚像素角点坐标
                corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
                # 绘制并显示角点
                cv2.drawChessboardCorners(
                    cam, (numCorner1, numCorner2), corners2, rets)
            cv2.imshow('Video1', cam)

            key = cv2.waitKey(1)
            # 按‘s’健保存图片
            if (key & 0xFF == ord('s')):
                if rets == True:
                    strIdImage = str(idImage)
                    cv2.imwrite(savePath + '/' + strIdImage + '.png', frame)
                    print('第{}张图片，保存成功'.format(idImage))
                    idImage = idImage+1
                else:
                    print('保存失败！棋盘格不完整，请换个角度重新保存！')

            # 按'ESC'退出程序
            if key & 0xFF == 27:
                print('程序已终止！一共保存了{}张图片'.format(idImage))
                break

    # 释放摄像头
    cap.release()
    cv2.destroyAllWindows()


if __name__ == '__main__':

    print('Starting the Calibration. Press and maintain the ESC key to exit the script\n')
    print('Push (s) to save the image')
    path = makeDir(numCam=2)
    # path = makeDir(numCam=1)
    if path:
        # getPicture(numCam=1, index=1, savePath=path)
        getPicture(numCam=2, index=0, savePath=path, width=2560,height=720)

capture.py
jiayuzhang128/StereoMeasurement
import numpy as np
import cv2
from openpyxl import Workbook # Used for writing data into an Excel file
from sklearn.preprocessing import normalize
from calibrationStore import loadStereoCoefficients

# Filtering
kernel= np.ones((3,3),np.uint8)

def coords_mouse_disp(event,x,y,flags,param):
    if event == cv2.EVENT_LBUTTONDBLCLK:
        #print x,y,disp[y,x],filteredImg[y,x]
        average=0
        for u in range (-1,2):
            for v in range (-1,2):
                average += disp[y+u,x+v]
        average=average/9
        Distance= -593.97*average**(3) + 1506.8*average**(2) - 1373.1*average + 522.06
        Distance= np.around(Distance*0.01,decimals=2)
        print('Distance: '+ str(Distance)+' m')
        
# This section has to be uncommented if you want to take mesurements and store them in the excel
##        ws.append([counterdist, average])
##        print('Measure at '+str(counterdist)+' cm, the dispasrity is ' + str(average))
##        if (counterdist <= 85):
##            counterdist += 3
##        elif(counterdist <= 120):
##            counterdist += 5
##        else:
##            counterdist += 10
##        print('Next distance to measure: '+str(counterdist)+'cm')

# Mouseclick callback
wb=Workbook()
ws=wb.active  

# #*************************************************
# #***** Parameters for Distortion Calibration *****
# #*************************************************

# # Termination criteria
# 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)

# # Prepare object points
# objp = np.zeros((9*6,3), np.float32)
# objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)

# # Arrays to store object points and image points from all images
# objpoints= []   # 3d points in real world space
# imgpointsR= []   # 2d points in image plane
# imgpointsL= []

# # Start calibration from the camera
# print('Starting calibration for the 2 cameras... ')
# # Call all saved images
# for i in range(0,67):   # Put the amount of pictures you have taken for the calibration inbetween range(0,?) wenn starting from the image number 0
#     t= str(i)
#     ChessImaR= cv2.imread('chessboard-R'+t+'.png',0)    # Right side
#     ChessImaL= cv2.imread('chessboard-L'+t+'.png',0)    # Left side
#     retR, cornersR = cv2.findChessboardCorners(ChessImaR,
#                                                (9,6),None)  # Define the number of chees corners we are looking for
#     retL, cornersL = cv2.findChessboardCorners(ChessImaL,
#                                                (9,6),None)  # Left side
#     if (True == retR) & (True == retL):
#         objpoints.append(objp)
#         cv2.cornerSubPix(ChessImaR,cornersR,(11,11),(-1,-1),criteria)
#         cv2.cornerSubPix(ChessImaL,cornersL,(11,11),(-1,-1),criteria)
#         imgpointsR.append(cornersR)
#         imgpointsL.append(cornersL)

# # Determine the new values for different parameters
# #   Right Side
# retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(objpoints,
#                                                         imgpointsR,
#                                                         ChessImaR.shape[::-1],None,None)
# hR,wR= ChessImaR.shape[:2]
# OmtxR, roiR= cv2.getOptimalNewCameraMatrix(mtxR,distR,
#                                                    (wR,hR),1,(wR,hR))

# #   Left Side
# retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(objpoints,
#                                                         imgpointsL,
#                                                         ChessImaL.shape[::-1],None,None)
# hL,wL= ChessImaL.shape[:2]
# OmtxL, roiL= cv2.getOptimalNewCameraMatrix(mtxL,distL,(wL,hL),1,(wL,hL))

# print('Cameras Ready to use')

# #********************************************
# #***** Calibrate the Cameras for Stereo *****
# #********************************************

# # StereoCalibrate function
# #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 = criteria_stereo,
#                                                           flags = cv2.CALIB_FIX_INTRINSIC)

# # StereoRectify function
# rectify_scale= 0 # if 0 image croped, if 1 image nor croped
# RL, RR, PL, PR, Q, roiL, roiR= cv2.stereoRectify(MLS, dLS, MRS, dRS,
#                                                  ChessImaR.shape[::-1], R, T,
#                                                  rectify_scale,(0,0))  # last paramater is alpha, if 0= croped, if 1= not croped
# # initUndistortRectifyMap function
# Left_Stereo_Map= cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
#                                              ChessImaR.shape[::-1], cv2.CV_16SC2)   # cv2.CV_16SC2 this format enables us the programme to work faster
# Right_Stereo_Map= cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
#                                               ChessImaR.shape[::-1], cv2.CV_16SC2)


#*******************************************
#***** Parameters for the StereoVision *****
#*******************************************

# Create StereoSGBM and prepare all parameters
window_size = 3
min_disp = 2
num_disp = 130-min_disp
stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
    numDisparities = num_disp,
    blockSize = window_size,
    uniquenessRatio = 10,
    speckleWindowSize = 100,
    speckleRange = 32,
    disp12MaxDiff = 12,
    P1 = 8*3*window_size**2,
    P2 = 32*3*window_size**2)

# Used for the filtered image
stereoR=cv2.ximgproc.createRightMatcher(stereo) # Create another stereo for right this time

# WLS FILTER Parameters
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)

_, _, _, _, _, _, _, _, _, _, _, _, _, _, _, _, _, mapLx, mapLy,mapRx, mapRy = loadStereoCoefficients("./RectifyStereoCalibParam.yml", True)
#*************************************
#***** Starting the StereoVision *****
#*************************************

# Call the two cameras
cap = cv2.VideoCapture(0)   # Wenn 0 then Right Cam and wenn 2 Left Cam
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)     # 宽度
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)   # 高度
# CamL= cv2.VideoCapture(2)

while True:

    ret, frame = cap.read()
    # cv2.imshow("show",frame)
    # cv2.waitKey(0)
    frameL = frame[0:720,0:1280]
    frameR = frame[0:720,1280:2560]
    frameLR = cv2.hconcat([frameL,frameR])
    camL = frameL.copy()
    camR = frameR.copy()
    # Start Reading Camera images
    # retR, frameR= CamR.read()
    # retL, frameL= CamL.read()

    # Rectify the images on rotation and alignement
    Left_nice= cv2.remap(frameL,mapLx,mapLy, interpolation = cv2.INTER_LINEAR)  # Rectify the image using the kalibration parameters founds during the initialisation
    Right_nice= cv2.remap(frameR,mapRx,mapRy, interpolation = cv2.INTER_LINEAR)

##    # Draw Red lines
##    for line in range(0, int(Right_nice.shape[0]/20)): # Draw the Lines on the images Then numer of line is defines by the image Size/20
##        Left_nice[line*20,:]= (0,0,255)
##        Right_nice[line*20,:]= (0,0,255)
##
##    for line in range(0, int(frameR.shape[0]/20)): # Draw the Lines on the images Then numer of line is defines by the image Size/20
##        frameL[line*20,:]= (0,255,0)
##        frameR[line*20,:]= (0,255,0)    
        
    # Show the Undistorted images
    #cv2.imshow('Both Images', np.hstack([Left_nice, Right_nice]))
    #cv2.imshow('Normal', np.hstack([frameL, frameR]))

    # Convert from color(BGR) to gray
    grayR= cv2.cvtColor(Right_nice,cv2.COLOR_BGR2GRAY)
    grayL= cv2.cvtColor(Left_nice,cv2.COLOR_BGR2GRAY)

    # Compute the 2 images for the Depth_image
    disp= stereo.compute(grayL,grayR)#.astype(np.float32)/ 16
    dispL= disp
    dispR= stereoR.compute(grayR,grayL)
    dispL= np.int16(dispL)
    dispR= np.int16(dispR)

    # Using the WLS filter
    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)
    #cv2.imshow('Disparity Map', filteredImg)
    disp= ((disp.astype(np.float32)/ 16)-min_disp)/num_disp # Calculation allowing us to have 0 for the most distant object able to detect

##    # Resize the image for faster executions
##    dispR= cv2.resize(disp,None,fx=0.7, fy=0.7, interpolation = cv2.INTER_AREA)

    # Filtering the Results with a closing filter
    closing= cv2.morphologyEx(disp,cv2.MORPH_CLOSE, kernel) # Apply an morphological filter for closing little "black" holes in the picture(Remove noise) 

    # Colors map
    dispc= (closing-closing.min())*255
    dispC= dispc.astype(np.uint8)                                   # Convert the type of the matrix from float32 to uint8, this way you can show the results with the function cv2.imshow()
    disp_Color= cv2.applyColorMap(dispC,cv2.COLORMAP_HSV)         # Change the Color of the Picture into an Ocean Color_Map
    filt_Color= cv2.applyColorMap(filteredImg,cv2.COLORMAP_HSV) 

    # Show the result for the Depth_image
    #cv2.imshow('Disparity', disp)
    #cv2.imshow('Closing',closing)
    #cv2.imshow('Color Depth',disp_Color)
    # cv2.imshow('Filtered Color Depth',filt_Color)
    colorFilt = cv2.vconcat([filt_Color, frameL])
    cv2.imshow("result", colorFilt)

    # Mouse click
    # cv2.setMouseCallback("Filtered Color Depth",coords_mouse_disp,filt_Color)
    cv2.setMouseCallback("result",coords_mouse_disp,colorFilt)
    
    # End the Programme
    if cv2.waitKey(1) & 0xFF == 27:
        break
    
# Save excel
##wb.save("data4.xlsx")

# Release the Cameras
cap.release()
# CamL.release()
cv2.destroyAllWindows()


main.py
jiayuzhang128/StereoMeasurement
import numpy as np
import cv2
import glob
import argparse
from calibrationStore import saveCoefficients

# 终止条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)


def monoCalib(dirPath, prefix, imageFormat, saveFile, squareSize, width=8, height=5):
    """ 单目标定函数 """
    # 生成真实角点相对坐标 (0,0,0), (1,0,0), (2,0,0) ....,(8,6,0)
    objp = np.zeros((height*width, 3), np.float32)
    objp[:, :2] = np.mgrid[0:width, 0:height].T.reshape(-1, 2)
    # 生成真实角点绝对坐标
    objp = objp * squareSize

    # 储存所有图像中真实角点坐标和图像角点坐标
    objpoints = []  # 世界坐标系3d点
    imgpoints = []  # 图像坐标系2d点

    # 路径校正，确保最后一个字符不是'/'
    if dirPath[-1:] == '/':
        dirPath = dirPath[:-1]

    # 获得图片
    images = glob.glob(dirPath+'/' + prefix + '*.' + imageFormat)
    # print(images)

    # 迭代遍历所有图像，将每张图像角点的真实坐标和图像坐标添加到数组中
    for name in images:
        img = cv2.imread(name)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # 寻找棋盘格角点
        ret, corners = cv2.findChessboardCorners(gray, (width, height), None)

        if ret:
            objpoints.append(objp)

            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
            imgpoints.append(corners2)

            # 绘制并显示棋盘格角点
            img = cv2.drawChessboardCorners(img, (width, height), corners2, ret)
            # cv2.imshow(img)

    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
    
    saveCoefficients(saveFile, mtx, dist, ret)

    print("Calibration is finished. RMS: ", ret)

    return [ret, mtx, dist, rvecs, tvecs]


if __name__ == '__main__':
    # 使用'-h'查看各个参数的帮助
    parser = argparse.ArgumentParser(description='Camera calibration')
    parser.add_argument('--imageDir', type=str, required=False, default='CalibDataMono', help='image directory path')
    parser.add_argument('--imageFormat', type=str, required=False, default='png', help='image format, png/jpg')
    parser.add_argument('--prefix', type=str, required=False, default='', help='image prefix')
    parser.add_argument('--squareSize', type=float, required=False, default=20.0, help='chessboard square size')
    parser.add_argument('--width', type=int, required=False, default=11, help='chessboard width size, default is 11')
    parser.add_argument('--height', type=int, required=False, default=8, help='chessboard height size, default is 8')
    parser.add_argument('--saveFile', type=str, required=False, default='monoCalibParam.yml', help='YML file to save calibration matrices')

    args = parser.parse_args()

    # 调用标定函数. RMS是误差, 误差小于0.2为佳
    ret, mtx, dist, rvecs, tvecs = monoCalib(args.imageDir, args.prefix, args.imageFormat, args.saveFile,args.squareSize, args.width, args.height)

monoCalibration.py
jiayuzhang128/StereoMeasurement
import numpy as np
import cv2
import argparse
from monoCalibration import monoCalib
from calibrationStore import loadCoefficients, saveStereoCoefficients, loadStereoImages

# 终止条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
image_size = None


def stereoCalibrate(paramL, paramR, dirL, prefixL, dirR, prefixR, imageFormat, saveFile, squareSize, width=11, height=8):
    """ 双目标定和校正 """
    objp, leftp, rightp, imageSize = loadImagePoints(dirL, prefixL, dirR, prefixR, imageFormat, squareSize, width, height)
    
    # 获取单目标定参数
    if paramL and paramR: # 从文件中读取
        K1, D1 = loadCoefficients(paramL)
        K2, D2 = loadCoefficients(paramR)
    else: # 直接利用图片进行标定
        _,K1, D1, _, _ = monoCalib(dirPath=dirL, prefix=prefixL, imageFormat=imageFormat, saveFile='stereoCalibParamL.yml', squareSize=squareSize, width=width, height=height)
        _,K2, D2, _, _ = monoCalib(dirPath=dirR, prefix=prefixR, imageFormat=imageFormat, saveFile='stereoCalibParamR.yml', squareSize=squareSize, width=width, height=height)

    flag = 0
    # flag |= cv2.CALIB_FIX_INTRINSIC
    flag |= cv2.CALIB_USE_INTRINSIC_GUESS
    ret, K1, D1, K2, D2, R, T, E, F = cv2.stereoCalibrate(
        objp, leftp, rightp, K1, D1, K2, D2, imageSize)
    print("Stereo calibration RMS: ", ret)

    saveStereoCoefficients(saveFile, imageSize, K1, D1, K2, D2, R, T, E, F, ret)


def loadImagePoints(dirL, prefixL, dirR, prefixR, imageFormat, squareSize,width=11, height=8):
    pattern_size = (width, height)  # Chessboard size!
    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(8,6,0)
    objp = np.zeros((height * width, 3), np.float32)
    objp[:, :2] = np.mgrid[0:width, 0:height].T.reshape(-1, 2)

    objp = objp * squareSize  # Create real world coords. Use your metric.

    # Arrays to store object points and image points from all the images.
    objpoints = []  # 3d point in real world space
    imgpointsL = []  # 2d points in image plane.
    imgpointsR = []  # 2d points in image plane.

    # 加载立体图像
    pair_images = loadStereoImages(dirL,dirR,imageFormat)

    # Iterate through the pairs and find chessboard corners. Add them to arrays
    # If openCV can't find the corners in one image, we discard the pair.
    for left_im, right_im in pair_images:
        # Right Object Points
        right = cv2.imread(right_im)
        gray_right = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)

        # Find the chess board corners
        ret_right, corners_right = cv2.findChessboardCorners(gray_right, pattern_size,
                                                             cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS)

        # Left Object Points
        left = cv2.imread(left_im)
        gray_left = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)

        # Find the chess board corners
        ret_left, corners_left = cv2.findChessboardCorners(gray_left, pattern_size,
                                                           cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS)

        if ret_left and ret_right:
            # Object points
            objpoints.append(objp)
            # Right points
            corners2_right = cv2.cornerSubPix(gray_right, corners_right, (5, 5), (-1, -1), criteria)
            imgpointsR.append(corners2_right)
            # Left points
            corners2_left = cv2.cornerSubPix(gray_left, corners_left, (5, 5), (-1, -1), criteria)
            imgpointsL.append(corners2_left)
        else:
            print("Chessboard couldn't detected. Image pair: ", left_im, " and ", right_im)
            continue

    imageSize = gray_right.shape  # If you have no acceptable pair, you may have an error here.
    print(imageSize)
    return [objpoints, imgpointsL, imgpointsR, imageSize]

if __name__ == '__main__':
    # Check the help parameters to understand arguments
    parser = argparse.ArgumentParser(description='Stereo Calibration')
    parser.add_argument('--paramL', type=str, required=False, default='', help='left matrix YML file')
    parser.add_argument('--paramR', type=str, required=False, default='', help='right matrix YML file')
    parser.add_argument('--prefixL', type=str, required=False, default= '', help='left image prefix')
    parser.add_argument('--prefixR', type=str, required=False, default= '', help='right image prefix')
    parser.add_argument('--dirL', type=str, required=False, default='CalibDataStereo/left', help='left images directory path')
    parser.add_argument('--dirR', type=str, required=False, default='CalibDataStereo/right', help='right images directory path')
    parser.add_argument('--imageFormat', type=str, required=False, default='png', help='image format, png/jpg')
    parser.add_argument('--width', type=int, required=False, default=11, help='chessboard width size, default is 11')
    parser.add_argument('--height', type=int, required=False, default=8, help='chessboard height size, default is 8')
    parser.add_argument('--squareSize', type=float, required=False, default=20.0, help='chessboard square size')
    parser.add_argument('--saveFile', type=str, required=False, default='stereoCalibParam.yml', help='YML file to save stereo calibration matrices')

    args = parser.parse_args()
    # If chessboard pattern is different, we will pass them as arguments.
    if args.width is None and args.height is None:
        stereoCalibrate(args.paramL, args.paramR, args.dirL, args.dirR, args.prefixR, args.imageFormat, args.saveFile, args.squareSize)
    else:
        stereoCalibrate(args.paramL, args.paramR, args.dirL, args.prefixL, args.dirR, args.prefixR, args.imageFormat, args.saveFile, args.squareSize, args.width, args.height)
stereoCalibration.py
jiayuzhang128/StereoMeasurement
import cv2
import argparse
from calibrationStore import loadStereoCoefficients, saveStereoCoefficients, loadStereoImages
from capture import makeDir

def rectify(dirL, dirR, imageFormat, loadCalibFile, saveCalibFile):

    # 立体校正
    imageSize, K1, D1, K2, D2, R, T, E, F, rms = loadStereoCoefficients(loadCalibFile, rectifid=False)

    RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(K1, D1, K2, D2, imageSize, R, T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0, newImageSize=imageSize)

    # 校正映射
    mapLx, mapLy = cv2.initUndistortRectifyMap(K1, D1, RL, PL, imageSize, cv2.CV_32FC1)
    mapRx, mapRy = cv2.initUndistortRectifyMap(K2, D2, RR, PR, imageSize, cv2.CV_32FC1)

    saveStereoCoefficients(saveCalibFile,  imageSize, K1, D1, K2, D2, R, T, E, F, rms,RL, RR, PL, PR, Q, roiL, roiR, mapLx, mapLy, mapRx, mapRy)

    count = 0
    saveImagesPath = makeDir(numCam=2, path='RectifyData')

    print("开始图片校正，按‘c’继续，按‘ESC'退出\n")
    unrectifyImagesLR = loadStereoImages(dirL,dirR,imageFormat)

    for imageL,imageR in unrectifyImagesLR:
        unrectifyL = cv2.imread(imageL)
        unrectifyR = cv2.imread(imageR)

        rectifyL = cv2.remap(unrectifyL, mapLx, mapLy, cv2.INTER_LINEAR)
        rectifyR = cv2.remap(unrectifyR, mapRx, mapRy, cv2.INTER_LINEAR)

        # 保存结果
        cv2.imwrite(saveImagesPath + "/left/" + str(count) + ".png", rectifyL)
        cv2.imwrite(saveImagesPath + "/right/" + str(count) + ".png", rectifyR)
    
        # 显示结果
        showRectify(unrectifyL,unrectifyR,rectifyL,rectifyR)

        # 输出提示信息
        count += 1
        print("图片对：" + imageL + " " + imageR + " 校正成功")
        print("已校正成功校正 " + str(count) + " 对图片\n")

        key = cv2.waitKey(0)
        if key == 'c':
            continue
        elif key == 27:
            break

def showRectify(unrectifyL,unrectifyR,rectifyL,rectifyR):

    unrectifyLR = cv2.hconcat([unrectifyL, unrectifyR])
    rectifyLR = cv2.hconcat([rectifyL, rectifyR])

    cv2.rectangle(rectifyLR, (0,0), (1280,720),(255,0,0),3)
    cv2.rectangle(rectifyLR, (1280,0), (2560,720),(0,0,255),3)

    mixLR = cv2.vconcat([unrectifyLR,rectifyLR])

    h, w, _ = mixLR.shape

    scalar = 1200/max(h, w) # 缩放图像宽度为1200，方便可视化
    H = int(h * scalar)
    W = int(w * scalar)
    scaleMixLR = cv2.resize(mixLR, (W,H), interpolation=cv2.INTER_LINEAR)

    for i in range(0, H,16):
        cv2.line(scaleMixLR, (0,i), (W,i), (0,255,0))

    cv2.namedWindow("result",cv2.WINDOW_AUTOSIZE)
    cv2.imshow("result",scaleMixLR)


    
if __name__ == "__main__":

    parser = argparse.ArgumentParser(description='Stereo Rectify')
    parser.add_argument('--dirL', type=str, required=False, default='CalibDataStereo/left', help='left unrectified images directory path')
    parser.add_argument('--dirR', type=str, required=False, default='CalibDataStereo/right', help='right unrectified images directory path')
    parser.add_argument('--imageFormat', type=str, required=False, default='png', help='image format, png/jpg')
    parser.add_argument('--loadCalibFile', type=str, required=False, default='./stereoCalibParam.yml', help='name of stereo calibration data YML file')
    parser.add_argument('--saveCalibFile', type=str, required=False, default='./RectifyStereoCalibParam.yml', help='name of rectified YML file')

    args = parser.parse_args()

    rectify(args.dirL, args.dirR, args.imageFormat, args.loadCalibFile, args.saveCalibFile)
stereoRectify.py
jiayuzhang128/StereoMeasurement