Obstacle Distance Determination through Depth Mapping - Carleton-SRCL/SPOT GitHub Wiki

      ####################################################
  # stereo depth map functions.py
  # Author: Adrian Comisso
  # Date: 2024-02-26
  #
  # Description:
  #
  # usfull links
  # -main document
  # https://docs.opencv.org/3.4/dd/d53/tutorial_py_depthmap.html
  # -issue forums
  # https://answers.opencv.org/question/59032/disparity-map-issues/
  # -how to use on video feed blog
  # https://stereopi.com/blog/opencv-and-depth-map-stereopi-tutorial
  ####################################################
  
  ####################################################
  # Imports
  ####################################################
  import cv2
  import numpy as np
  import os
  from datetime import datetime
  from matplotlib import pyplot as plt
  
  
  ####################################################
  # Functions
  ####################################################
  
  # function to split an image into two halves
  def split_image(image):
      # check if the image is 2D or 3D
      if len(image.shape) == 2:
          # convert to 3D where each layer is Red, Green, and Blue
          image = np.expand_dims(image, axis=2)
      # dimensions of the image
      dimensions = image.shape
      height = dimensions[0]
      width = dimensions[1]
      midpoint = width // 2
      # split the image into two halves
      left_half = image[:, :midpoint, :]
      right_half = image[:, midpoint:, :]
      # return the two halves
      return left_half, right_half
  
  
  # function to resize an image for display
  def resize_for_display(image, scale_percent):
      # calculate the new dimensions using percentage
      width = int(image.shape[1] * scale_percent / 100)
      height = int(image.shape[0] * scale_percent / 100)
      # new dimension tuple
      dim = (width, height)
      # resize the image to the specified dimensions for display
      resized_image = cv2.resize(image, dim, interpolation=cv2.INTER_AREA)
      # retun the resized image
      return resized_image
  
  
  # function to save an image with a unique name
  def save_image(image, filename):
      # get the current date and time
      timestamp = datetime.now().strftime("%Y%m%d%H%M%S")
      # create a unique filename
      filename = f"{filename}_{timestamp}.png"
      # generate the output path
      output_path = os.path.join(output_file_path, filename)
      # save the image
      cv2.imwrite(output_path, image)
      # notify the user
      print(f"Saved: {output_path}")
  
  
  # function to crop the top of an image
  def crop_top(image, num_rows):
      height, width = image.shape[:2]
      cropped_image = image[num_rows:height, :]
      return cropped_image
  
  
  # function to crop the bottom of an image
  def crop_bottom(image, num_rows):
      height, width = image.shape[:2]
      cropped_image = image[0:height - num_rows, :]
      return cropped_image
  
  
  # function to rotate image by angle
  def rotate_image(image, angle):
      # get the dimensions of the image
      height, width = image.shape[:2]
      # get the center of the image
      center = (width / 2, height / 2)
      # get the rotation matrix
      rotation_matrix = cv2.getRotationMatrix2D(center, angle, 1.0)
      # rotate the image
      rotated_image = cv2.warpAffine(image, rotation_matrix, (width, height))
      # return the rotated image
      return rotated_image
  
  
  ####################################################
  # Main Function
  ####################################################
  
  ##### Main Function #####
  def find_obstacle_depth(un_dist_img, bound_left, bound_top, bound_right, bound_bottom, show_images, show_disparity,
                          show_depth, show_points):
      ##### Inputs #####
      # color layer to use
      color_layer = 'red'
      # bounding box parameters
      step_size = 20
      # point selection padding
      padding = 20
  
      ##### Correct Tilt #####
      # split left and right images
      left_img = split_image(un_dist_img)[0]
      right_img = split_image(un_dist_img)[1]
      # amount to crop top and bottom of image to acive alignment
      pix_to_crop = 36  # ********************************************************************************
      # crop top and bottom of respective images to align rows
      right_img = crop_bottom(right_img, pix_to_crop)
      left_img = crop_top(left_img, pix_to_crop)
      # resize images for undistort
      # left_img = cv2.resize(left_img, (1280, 720))
      # right_img = cv2.resize(right_img, (1280, 720))
      if show_images:
          cv2.imshow('Left Image', resize_for_display(left_img, 50))
          cv2.imshow('Right Image', resize_for_display(right_img, 50))
  
      ##### Convert to Grey Scale #####
      # extract R, G, and B layers
      bl, gl, rl = cv2.split(left_img)
      br, gr, rr = cv2.split(right_img)
      # choose which layer to use
      if color_layer == 'red':
          left_img = rl
          right_img = rr
      elif color_layer == 'green':
          left_img = gl
          right_img = gr
      elif color_layer == 'blue':
          left_img = bl
          right_img = br
      else:  # grey scale
          left_img = cv2.cvtColor(left_img, cv2.COLOR_RGB2GRAY)
          right_img = cv2.cvtColor(right_img, cv2.COLOR_RGB2GRAY)
  
      ##### Compute Disparity Map #####
      # size of the window that gets scanned
      window_size = 3
      # create stereo disparity object
      stereo = cv2.StereoSGBM_create(minDisparity=16,  # min possible disparity (multiple of 16)
                                     numDisparities=16 * 14,  # forget what this does ****18
                                     blockSize=window_size,  # size of the window that gets scanned
                                     uniquenessRatio=5,  # how uniwue can a section be before it is discarded
                                     disp12MaxDiff=2,  # dunno
                                     speckleWindowSize=10,  # window for noise removal
                                     speckleRange=2,  # range to remove noise
                                     P1=8 * 3 * window_size ** 2,  # cost fucntion for moving vertically
                                     P2=32 * 3 * window_size ** 2,  # cost fucntion for moving horizontally
                                     mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)  # mode for the stereo disparity object
      # compute the disparity map
      disparity = stereo.compute(left_img, right_img).astype(np.float32) / 16.0
      # show disparity map
      if show_disparity:
          plt.imshow(disparity, 'gray')
          plt.show()
  
      ##### Compute Depth Map #####
      # camera parameters
      focal_length = 1080  # 694 #in pixels #1419
      baseline = 0.12  # in m
      # #compute depth map
      # for i in range(disparity.shape[0]):  # Loop through rows
      #     for j in range(disparity.shape[1]):  # Loop through columns
      #         # Apply your calculation to each pixel
      #         disparity[i, j] = focal_length*baseline/((disparity[i, j])) #curve fit conversion
      # #show depth map
      # if show_depth:
      #     plt.imshow(disparity,'gray')
      #     plt.show()
  
      ##### Compute Average Depth of Bounding Box #####
      # initialize loop variables
      sum = 0
      count = 0
      # find depth of obstacle
      for i in range(bound_top + padding, bound_bottom - padding, step_size):
          for j in range(bound_left + padding, bound_right - padding, step_size):
              # Apply your calculation to each pixel
              disparity[i, j] = focal_length * baseline / ((disparity[i, j]))
              if disparity[i, j] < 2.75:
                  sum += disparity[i, j]
                  count += 1
              disparity[i, j] = 10
      # show depth map with points
      if show_points:
          plt.imshow(disparity, 'gray')
          plt.show()
      # calculate average depth
      if count == 0:
          depth = int(1000)
      else:
          depth = sum / count
      return depth