Optimized ZED Camera Calibration - Carleton-SRCL/SPOT GitHub Wiki

  """
  This is technically version three of stero calibration for the computer vision team, dubbed Clear Cut Stereo (CCS)
  
  This version is a synopsis of CalibrationProcedure,py, stereovision_calibration.py and finally StereoVision_cal_2.py
  
  It is meant to be simple, clear and effective for the production of the needed calibration files.
  Then have supplementary error analysis for calculations for a full diagnostic.
  
  Written by Hayden Arms, November 8th, 2023.
  
  """
  
  # Import needed toolboxes
  import numpy as np
  import cv2 as cv
  import glob
  import matplotlib.pyplot as plt
  import os
  
  
  def main():
      # Calibration Settings
      squareSize = 18  # measured square size in world units (mm)
      chessboardSize = (14, 9)  # size of checker board used (number of squares in col and rows +1)
      Export = True
      ExportDirectory = r'/On table Cal'
      ImageDirectoryR = r'/*.png'  # directory to cal images
      ImageDirectoryL = r'/*.png'
      FullDiagnostic = True  # Config T/F to run full error diagnositc, set to false to simply output the stereo calibration files
  
      # Full diagnostic Settings
      isVerticalStereo = False  # left-right or up-down stereo camera arrangement
      useCalibrated = True  # calibrated or uncalibrated stereo rectification
  
      # CCS Main Body, Stereo Calibration File Generation and Export.
      imgesL, imgesR, LimgNames, RimgNames = ImageRead(chessboardSize, ImageDirectoryR, ImageDirectoryL)  # Read in Images
  
      # First analyize the checkerboard images for corner positions
      frameSize, objpoints, cornersL, imgpointsL, cornersR, imgpointsR = \
          FindChessCorners(imgesL, imgesR, FullDiagnostic, squareSize, chessboardSize)  # Find corners of checkerboard 
  
      # calibrate each camera individually for initial guess`
      ML_i, DL_i, MR_i, DR_i = InitialSingleCamCal(objpoints, imgpointsL, imgpointsR, frameSize, FullDiagnostic)
  
      # Complete Stereo Calibration using initial camera guesses
      retRMS, CamMatL, DistMatL, CamMatR, DistMatR, RotMat, TranslVec, EssenM, FundM = StereoCal(objpoints, imgpointsL, imgpointsR, ML_i, DL_i, MR_i, DR_i, frameSize, FullDiagnostic)
  
      print("Returned RMS Error for Stero Cal: {}".format(retRMS))
  
      # Export Files
      if Export == True:
          np.save(os.path.join(ExportDirectory, "Left_calibration_matrix"), CamMatL)
          np.save(os.path.join(ExportDirectory, "Left_distortion_coefficients"), DistMatL)
          np.save(os.path.join(ExportDirectory, "Right_calibration_matrix"), CamMatR)
          np.save(os.path.join(ExportDirectory, "Right_distortion_coefficients"), DistMatR)
          np.save(os.path.join(ExportDirectory, "TranslationVector"), TranslVec)
          np.save(os.path.join(ExportDirectory, "RotationVector"), RotMat)
          # camera_model = dict([('ML', CamMatL), ('MR', CamMatR), ('distL', DistMatL),
          #                     ('distR', DistMatR), ('R', RotMat), ('T', TranslVec)])
          # np.save(os.path.join(ExportDirectory,"Camera_model"),camera_model)
  
      # Now enter full diagnostic section
      if FullDiagnostic == True:
          N = len(imgesL)
          mean_error_alt = calc_rms_stereo(objpoints, imgpointsL, imgpointsR, CamMatL, DistMatL, CamMatR, DistMatR, RotMat, TranslVec, LimgNames, RimgNames)
          print("Alternative Error Calculation: {}".format(mean_error_alt))
          # DisplayRectifiedImage(CamMatL, DistMatL, CamMatR, DistMatR, frameSize,RotMat, TranslVec)
          # DisparityMap()
  
  
  def DisparityMap():
      pass
  
  
  def DisplayRectifiedImage(Cam_L, Dist_L, Cam_R, Dist_R, frmSz, RotVec, TranslVec):
      RCT = cv.stereoRectify(Cam_L, Dist_L, Cam_R, Dist_R, frmSz, RotVec, TranslVec, 'ZeroDisparity', True, 'Alpha',
                             -1)  # 'Alpha',1
  
      # OpenCV can handle left-right or up-down camera arrangements
      isVerticalStereo = abs(2) > abs(1)
  
  
  def calc_rms_stereo(objectpoints, imgpoints_l, imgpoints_r, A1, D1, A2, D2, R, T, LframeName, RframeName):
      tot_error = 0
      total_points = 0
      frame_error_L = np.ones(len(imgpoints_l))
      frame_error_R = np.ones(len(imgpoints_r))
  
      for i, objpoints in enumerate(objectpoints):
          # calculate world <-> cam1 transformation
          _, rvec_l, tvec_l, _ = cv.solvePnPRansac(objpoints, imgpoints_l[i], A1, D1)
  
          # compute reprojection error for cam1
          rp_l, _ = cv.projectPoints(objpoints, rvec_l, tvec_l, A1, D1)
          l_er = np.sum(np.square(np.float64(imgpoints_l[i] - rp_l)))
          tot_error += l_er
          total_points += len(objpoints)
          frame_error_L[i] = np.sqrt(l_er / len(objpoints))
  
          # calculate world <-> cam2 transformation
          rvec_r, tvec_r = cv.composeRT(rvec_l, tvec_l, cv.Rodrigues(R)[0], T)[:2]
  
          # compute reprojection error for cam2
          rp_r, _ = cv.projectPoints(objpoints, rvec_r, tvec_r, A2, D2)
          r_er = np.sum(np.square(imgpoints_r[i] - rp_r))
          tot_error += r_er
          total_points += len(objpoints)
          frame_error_R[i] = np.sqrt(r_er / len(objpoints))
  
      mean_error = np.sqrt(tot_error / total_points)
  
      Nrng = np.arange(len(frame_error_L))
      f, ax = plt.subplots(1, sharex=True)
      ax.bar(Nrng - 0.2, frame_error_L, width=0.3, color='#F5E338', edgecolor='k', label="Left Feed")
      ax.bar(Nrng + 0.2, frame_error_R, width=0.3, color='#4916F5', edgecolor='k', label="Right Feed")
      ax.axhline(y=mean_error, color='r', linewidth=2, linestyle='--', label="Mean error")
      ax.legend(loc="upper right")
      ax.set_title('Individual Frame Error')
      plt.xlabel('Image Pairs')
      plt.ylabel('RMS Error (Pixel Units)')
  
      print("Image  - Error Correlations")
      frame_error_R = np.transpose([frame_error_R])
      frame_error_L = np.transpose([frame_error_L])
      RframeName = np.transpose([RframeName])
      LframeName = np.transpose([LframeName])
      fullList_R = np.hstack((np.array(frame_error_R, dtype=object), np.array(RframeName, dtype=object)))
      fullList_L = np.hstack((np.array(frame_error_L, dtype=object), np.array(LframeName, dtype=object)))
      print("Left Feed")
      print(fullList_L)
      print("Right Feed")
      print(fullList_R)
  
      return mean_error
  
  
  def ImageRead(chessSz, ImageDirR, ImageDirL):
      # Import Images
      imgesL = sorted(glob.glob(ImageDirL))  # import each image set for each camera lens
      imgesR = sorted(glob.glob(ImageDirR))
      assert len(imgesL) != 0 and len(imgesL) == len(imgesR), "Not an equal amount of left and right images."
  
      N = len(imgesL)  # number of calibration images
      h_img, w_img, _ = cv.imread(imgesL[0]).shape  # image size
  
      # sorted names
      leftImgNames = []
      rightImgNames = []
      for id in range(N):
          leftImgNames.append(imgesL[id].rsplit('/', 1)[1])
          rightImgNames.append(imgesR[id].rsplit('/', 1)[1])
  
      print('{} pairs of stereo images\n'.format(N))
      print('Full Feed Frame Size = {}x{}\n'.format(h_img, w_img))
      print('Chessboard Pattern size (nx,ny) = {}x{}\n'.format(chessSz[0], chessSz[1]))
      return imgesL, imgesR, leftImgNames, rightImgNames
  
  
  def FindChessCorners(imgesL, imgesR, FD, sqSz, chessSz):
      criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 1000, 1e-5)  # termination criteria for subpix
      # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
      objp = np.zeros((chessSz[0] * chessSz[1], 3), np.float32)
      objp[:, :2] = np.mgrid[0:chessSz[0], 0:chessSz[1]].T.reshape(-1, 2)
      objp = objp * sqSz  # in mm
      # 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.
      print("Determining Chessboard Corners for Left and Right feed inputs \n")
      for imgLeft, imgRight in zip(imgesL, imgesR):  # for each image (back and forth in left and right)
          imgL = cv.imread(imgLeft)
          imgR = cv.imread(imgRight)
          gray_img_L = cv.cvtColor(imgL, cv.COLOR_BGR2GRAY)
          gray_img_R = cv.cvtColor(imgR, cv.COLOR_BGR2GRAY)
  
          # Find the chess board corners
          retL, cornersL = cv.findChessboardCorners(gray_img_L, (chessSz[0], chessSz[1]), None)
          retR, cornersR = cv.findChessboardCorners(gray_img_R, (chessSz[0], chessSz[1]), None)
  
          # If found, add object points, image points (after refining them)
          if retL and retR:  # if both detected
              objpoints.append(objp)
              corners2L = cv.cornerSubPix(gray_img_L, cornersL, (11, 11), (-1, -1), criteria)
              corners2R = cv.cornerSubPix(gray_img_R, cornersR, (11, 11), (-1, -1), criteria)
              imgpointsL.append(corners2L)
              imgpointsR.append(corners2R)
              if FD == True:
                  # Draw and display the corners
                  cv.drawChessboardCorners(imgL, chessSz, cornersL, retL)  # draw em on
                  cv.drawChessboardCorners(imgR, chessSz, cornersR, retR)
                  Hori = np.concatenate((imgL, imgR), axis=1)
                  cv.imshow('HORIZONTAL', Hori)
                  cv.waitKey(300)  # manual wait
          else:
              print("Failed Detection")
  
      print("Determination Completed.\n Right Camera Points Found: {} \n Left Camera Points Found: {} \n".format(len(imgpointsR), len(imgpointsL)))
      cv.destroyAllWindows()
      frameSize = gray_img_L.shape[::-1]
      print("Single Feed Frame Size: {}".format(frameSize))
      return frameSize, objpoints, cornersL, imgpointsL, cornersR, imgpointsR
  
  
  def InitialSingleCamCal(opnts, pntsL, pntsR, frmSz, FD):
      print("Calibrating individual cameras... \n")
      # Calibrate each frame
      ret_L, mtx_L, dist_L, rvecs_L, tvecs_L = cv.calibrateCamera(opnts, pntsL, frmSz, None, None)
      newCameraMatrixL, roi_L = cv.getOptimalNewCameraMatrix(mtx_L, dist_L, frmSz, 1, frmSz)
      ret_R, mtx_R, dist_R, rvecs_R, tvecs_R = cv.calibrateCamera(opnts, pntsR, frmSz, None, None)
      newCameraMatrixR, roi_L = cv.getOptimalNewCameraMatrix(mtx_R, dist_R, frmSz, 1, frmSz)
      if FD == True:
          print("Reprojection error LEFT opencv:{} ".format(ret_L))
          print("Reprojection error RIGHT opencv:{} ".format(ret_R))
          # These are effecttively our initial guesses for each input frame
      print("Initial Guess Completed.\n")
      return newCameraMatrixL, dist_L, newCameraMatrixR, dist_R
  
  
  def StereoCal(objpoints, imgPL, imgPR, CamMatL, distL, CamMatR, distR, frmSz, FD):
      print("Beginning Stereo Calibration")
      criteria_stereo = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 1000, 1e-5)
      ster_flags = 0
      ster_flags |= cv.CALIB_SAME_FOCAL_LENGTH  # to true
      ster_flags |= cv.CALIB_FIX_ASPECT_RATIO  # to true
      ster_flags |= cv.CALIB_RATIONAL_MODEL
      ster_flags |= cv.CALIB_FIX_K3
      ster_flags |= cv.CALIB_FIX_K4
      ster_flags |= cv.CALIB_FIX_K5
  
      if FD == True:
          print("Flags Total:")
          print(ster_flags)
          print("Criteria:")
          print(criteria_stereo)
      # objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize
      retRMS, ML_s, dL_s, MR_s, dR_s, RotMat, TranslVec, EssenM, FundM = cv.stereoCalibrate(objpoints, imgPL, imgPR, CamMatL, distL, CamMatR, distR, frmSz, criteria=criteria_stereo, flags=ster_flags)
  
      return retRMS, ML_s, dL_s, MR_s, dR_s, RotMat, TranslVec, EssenM, FundM
  
  
  if __name__ == "__main__":
      main()
      plt.show()