完全python实现双目标定、测距

# ▄▀▄ ▄▀▄ # ▄█░░▀▀▀▀▀░░█▄ # ▄▄ █░░░░░░░░░░░█ ▄▄ #█▄▄█ █░░▀░░┬░░▀░░█ █▄▄█ ################################### ##### 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, OmtxL, distL, OmtxR, distR, ChessImaR.shape[::-1], criteria_stereo, flags) # 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 = 11 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], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0) # Rectify the image using the kalibration parameters founds during the initialisation Right_nice= cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0) ## # Draw Red lines ## for line in rang