Project 1: Stereo Based Scene Segmentation

#Importing the libraries
import cv2
import numpy as np
from matplotlib import pyplot as plt

# Set the path to the images captured by the left and right cameras
pathL = "./"
pathR = "./"
 
# Termination criteria for refining the detected corners
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
objp = np.zeros((9*6,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)
 
img_ptsL = []
img_ptsR = []
obj_pts = []


imgL = cv2.imread(pathL+"check0.png")
imgR = cv2.imread(pathR+"check1.png")
imgL_gray = cv2.imread(pathL+"check0.png",0)
imgR_gray = cv2.imread(pathR+"check1.png",0)


outputL = imgL.copy()
outputR = imgR.copy()

retR, cornersR =  cv2.findChessboardCorners(outputR,(9,6),None)
retL, cornersL = cv2.findChessboardCorners(outputL,(9,6),None)


if retR and retL:
    obj_pts.append(objp)
    cv2.cornerSubPix(imgR_gray,cornersR,(11,11),(-1,-1),criteria)
    cv2.cornerSubPix(imgL_gray,cornersL,(11,11),(-1,-1),criteria)
    cv2.drawChessboardCorners(outputR,(9,6),cornersR,retR)
    cv2.drawChessboardCorners(outputL,(9,6),cornersL,retL)
    cv2.imshow('cornersR',outputR)
    cv2.imshow('cornersL',outputL)
    cv2.waitKey(0)

    img_ptsL.append(cornersL)
    img_ptsR.append(cornersR) 
        
#Calibrating left camera
retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(obj_pts,img_ptsL,imgL_gray.shape[::-1],None,None)
hL,wL= imgL_gray.shape[:2]
new_mtxL, roiL= cv2.getOptimalNewCameraMatrix(mtxL,distL,(wL,hL),1,(wL,hL))
print("Left Camera Matrix:", mtxL)

# Calibrating right camera
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(obj_pts,img_ptsR,imgR_gray.shape[::-1],None,None)
hR,wR= imgR_gray.shape[:2]
new_mtxR, roiR= cv2.getOptimalNewCameraMatrix(mtxR,distR,(wR,hR),1,(wR,hR))
print("Right Camera Matrix:", mtxR)


#Stereo calibration with fixed intrinsic parameters
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
# Here we fix the intrinsic camara matrixes so that only Rot, Trns, Emat and Fmat are calculated.
# Hence intrinsic parameters are the same 
 
criteria_stereo= (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
#This step is performed to transformation between the two cameras and calculate Essential and Fundamenatl matrix
retS, new_mtxL, distL, new_mtxR, distR, Rot, Trns, Emat, Fmat = cv2.stereoCalibrate(obj_pts, img_ptsL, img_ptsR, new_mtxL, distL, new_mtxR, distR, imgL_gray.shape[::-1], criteria_stereo, flags)

#New Camera matrix
print("New Left Camera Matrix: ",new_mtxL)
print("New Right Camera Matrix: ",new_mtxR)

#Stereo Rectification
rectify_scale= 1
rect_l, rect_r, proj_mat_l, proj_mat_r, Q, roiL, roiR= cv2.stereoRectify(new_mtxL, distL, new_mtxR, distR, imgL_gray.shape[::-1], Rot, Trns, rectify_scale,(0,0))

# read two input images as grayscale images
imgL = cv2.imread('left.png',0)
imgR = cv2.imread('right.png',0)

# Initiate and StereoBM object
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=5)

# compute the disparity map
disparity = stereo.compute(imgL,imgR)
plt.imshow(disparity,'gray')
plt.show()
cv2.imwrite('disparity.png',disparity)
print(disparity)

#Depth map from Disparity map and Q
depth = cv2.reprojectImageTo3D(disparity, Q)
cv2.imwrite('Depth MAP.png',depth)
print(depth)